diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index 8c869eb..5b118c2 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -15,10 +15,12 @@ find_package(spatial_hash REQUIRED) find_package(Eigen3 REQUIRED) find_package(pose_graph_tools REQUIRED) find_package(GTSAM REQUIRED) -find_package(OpenCV REQUIRED COMPONENTS core imgproc) +find_package(OpenCV REQUIRED COMPONENTS core imgproc imgcodecs) find_package(spark_dsg REQUIRED) find_package(kimera_pgmo REQUIRED) find_package(hydra REQUIRED) +find_package(small_gicp REQUIRED) +find_package(CLI11 REQUIRED) include(GNUInstallDirs) @@ -26,8 +28,24 @@ include(GNUInstallDirs) # Libraries # ############# +add_library(${PROJECT_NAME}_registration + src/utils/icp_registration_utils.cpp +) +target_include_directories(${PROJECT_NAME}_registration + PUBLIC $ + $ +) +target_link_libraries(${PROJECT_NAME}_registration + PRIVATE small_gicp::small_gicp +) +set_property(TARGET ${PROJECT_NAME}_registration PROPERTY POSITION_INDEPENDENT_CODE ON) +add_library(khronos::${PROJECT_NAME}_registration ALIAS ${PROJECT_NAME}_registration) + add_library(${PROJECT_NAME} src/active_window/active_window.cpp + src/active_window/change_detection/active_window_change_detector.cpp + src/active_window/change_detection/transformation_getter.cpp + src/active_window/data/frame_data.cpp src/active_window/data/frame_data_buffer.cpp src/active_window/data/track.cpp src/active_window/integration/object_integrator.cpp @@ -37,6 +55,7 @@ add_library(${PROJECT_NAME} src/active_window/object_detection/instance_forwarding.cpp src/active_window/object_extraction/mesh_object_extractor.cpp src/active_window/object_extraction/object_worker_pool.cpp + src/active_window/track_saver_sink.cpp src/active_window/tracking/external_tracker.cpp src/active_window/tracking/max_iou_tracker.cpp src/backend/backend.cpp @@ -54,6 +73,7 @@ add_library(${PROJECT_NAME} src/spatio_temporal_map/spatio_temporal_map.cpp src/spatio_temporal_map/incremental_mesh.cpp src/utils/geometry_utils.cpp + src/utils/json_utils.cpp src/utils/khronos_attribute_utils.cpp src/utils/output_file_utils.cpp ) @@ -75,11 +95,39 @@ ${PROJECT_NAME} spatial_hash::spatial_hash ${kimera_pgmo_LIBRARIES} ${hydra_LIBRARIES} + ${PROJECT_NAME}_registration PRIVATE ${tf2_eigen_LIBRARIES} ) set_property(TARGET ${PROJECT_NAME} PROPERTY POSITION_INDEPENDENT_CODE ON) add_library(khronos::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) +############### +# Executables # +############### + +# Standalone round-trip check for Track::save/Track::load (see tracker_debug.md Step 1). +add_executable(test_track_serialization app/test_track_serialization.cpp) +target_link_libraries(test_track_serialization PRIVATE ${PROJECT_NAME}) +install(TARGETS test_track_serialization RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + +# Replays a saved track's observations through MaxIoUTracker against another loaded track (see +# tracker_debug.md Step 2). +add_executable(test_track_association app/test_track_association.cpp) +target_link_libraries(test_track_association PRIVATE ${PROJECT_NAME} CLI11::CLI11) +install(TARGETS test_track_association RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + +# Standalone round-trip check for FrameData::save/FrameData::load and +# saveCameraIntrinsics/loadCameraIntrinsics (see tracker_debug.md Step 3). +add_executable(test_frame_data_serialization app/test_frame_data_serialization.cpp) +target_link_libraries(test_frame_data_serialization PRIVATE ${PROJECT_NAME}) +install(TARGETS test_frame_data_serialization RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + +# Replays an entire saved run through a fresh MaxIoUTracker, with active-window eviction +# simulation, for parameter tuning (see tracker_debug.md Step 4). +add_executable(full_run_tracker_replay app/full_run_tracker_replay.cpp) +target_link_libraries(full_run_tracker_replay PRIVATE ${PROJECT_NAME} CLI11::CLI11) +install(TARGETS full_run_tracker_replay RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) + ########## # Export # ########## diff --git a/khronos/app/full_run_tracker_replay.cpp b/khronos/app/full_run_tracker_replay.cpp new file mode 100644 index 0000000..9de608b --- /dev/null +++ b/khronos/app/full_run_tracker_replay.cpp @@ -0,0 +1,419 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +/** + * 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). + * + * ------------------------------------------------------------------------------------------- + * USAGE + * ------------------------------------------------------------------------------------------- + * + * Binary: install/khronos/bin/full_run_tracker_replay (after `colcon build --packages-select + * khronos`). Run with --help to see all flags. + * + * full_run_tracker_replay --run-dir --output-dir + * + * All tracker-config flags (--bbox-type, --min-semantic-iou, --min-cross-iou, + * --min-num-observations, --track-by, --map-window-radius, --min-range, --max-range) DEFAULT TO + * THE DEPLOYED CONFIG's values (dcist_launch_system/config/default_awcd/hydra.yaml), not + * MaxIoUTracker::Config's own in-code defaults -- so a bare invocation with no flags at all + * reproduces production out of the box. (This was not always true: earlier defaults silently + * mirrored the stricter in-code defaults, e.g. min_semantic_iou=0.5 vs. the deployed 0.25, and a + * run without explicit flags produced ~2x the real fragmentation purely from that mismatch -- see + * tracker_debug.md's usability-fix writeup. If you're pointing this at a run from a DIFFERENT + * experiment override with different tracker/map_window values, pass them explicitly: + * + * full_run_tracker_replay \ + * --run-dir --output-dir \ + * --bbox-type aabb --min-semantic-iou 0.25 --min-cross-iou 0.1 --min-num-observations 10 \ + * --track-by pixels --map-window-type spatial --map-window-radius 14.0 \ + * --min-range 0.05 --max-range 10.0 + * + * or deliberately override individual values to test whether a different config would have + * avoided a known fragmentation case (e.g. --min-semantic-iou 0.2, --track-by bounding_box). + * + * --pause-on-event blocks on stdin (press Enter to continue) only at frames where a track was + * created or evicted -- the moments actually worth inspecting -- instead of every frame. + * + * --save-overlay (off by default) additionally writes overlay_.png (color + red mask blend) + * into each track's output directory for every frame it is updated, for offline visual QA of + * which detections got associated into which track. Off by default since it adds one image write + * per track per frame, which adds up over a full run. + * + * Output layout: + * /tracks/track_/track.json # tracks still active when the run ended + * /tracks/track_/overlay_.png # with --save-overlay + * /archived_tracks/track_/track.json # tracks evicted during the run + * /archived_tracks/track_/overlay_.png # with --save-overlay (moves with the + * # track when it's evicted) + * track.json is a plain Track::save() file -- diffable against the original run's real tracks/ + * folder, and directly loadable by test_track_association for further investigation of any new + * fragmentation this replay surfaces. + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "khronos/active_window/data/frame_data.h" +#include "khronos/active_window/data/track.h" +#include "khronos/active_window/tracking/max_iou_tracker.h" + +using namespace khronos; + +namespace { + +std::vector sortedObservationStamps(const std::string& run_dir) { + std::vector stamps; + for (const auto& entry : std::filesystem::directory_iterator(run_dir + "/observations")) { + if (!entry.is_directory()) { + continue; + } + try { + stamps.push_back(static_cast(std::stoull(entry.path().filename().string()))); + } catch (const std::exception&) { + std::cerr << "Skipping non-timestamp observations/ entry: " << entry.path() << "\n"; + } + } + std::sort(stamps.begin(), stamps.end()); + return stamps; +} + +std::set trackIds(const Tracks& tracks) { + std::set ids; + for (const auto& track : tracks) { + ids.insert(track.id); + } + return ids; +} + +// Working directory for a track while it is still alive (whether it ends up still-active at run +// end or evicted partway through) -- also where --save-overlay images accumulate over the track's +// lifetime, so an evicted track's overlay history moves with it (see archiveTrack). +std::string trackWorkingDir(const std::string& output_dir, int track_id) { + return output_dir + "/tracks/track_" + std::to_string(track_id); +} + +void archiveTrack(const std::string& output_dir, const Track& track) { + const std::string working_dir = trackWorkingDir(output_dir, track.id); + const std::string archived_dir = + output_dir + "/archived_tracks/track_" + std::to_string(track.id); + std::filesystem::create_directories(output_dir + "/archived_tracks"); + if (std::filesystem::exists(working_dir)) { + // Move (not copy) so any overlay_.png written during the track's life moves with it, + // rather than being duplicated or left behind in tracks/. + std::error_code ec; + std::filesystem::rename(working_dir, archived_dir, ec); + if (ec) { + std::filesystem::create_directories(archived_dir); + } + } else { + std::filesystem::create_directories(archived_dir); + } + track.save(archived_dir + "/track.json"); +} + +// Writes overlay_.png (color image with the track's current-frame mask blended in red) into +// the track's working directory, for offline visualization. Only called when --save-overlay is +// set; mirrors ActiveWindowTrackSaver::saveOverlay's blending logic. +void saveOverlay(const std::string& output_dir, const Track& track, const FrameData& frame_data) { + if (track.observations.empty() || frame_data.input.color_image.empty()) { + return; + } + const Observation& obs = track.observations.back(); + + cv::Mat binary_mask; + if (obs.semantic_cluster_id >= 0 && !frame_data.object_image.empty()) { + binary_mask = (frame_data.object_image == obs.semantic_cluster_id); + } else if (obs.dynamic_cluster_id >= 0 && !frame_data.dynamic_image.empty()) { + binary_mask = (frame_data.dynamic_image == obs.dynamic_cluster_id); + } else { + return; + } + + const std::string dir = trackWorkingDir(output_dir, track.id); + std::filesystem::create_directories(dir); + + cv::Mat bgr_image; + cv::cvtColor(frame_data.input.color_image, bgr_image, cv::COLOR_RGB2BGR); + cv::Mat overlay = bgr_image.clone(); + overlay.setTo(cv::Scalar(0, 0, 255), binary_mask); // Red in BGR. + cv::Mat blended; + cv::addWeighted(bgr_image, 0.6, overlay, 0.4, 0, blended); + cv::imwrite(dir + "/overlay_" + std::to_string(obs.stamp) + ".png", blended); +} + +void saveActiveTrack(const std::string& output_dir, const Track& track) { + const std::string dir = trackWorkingDir(output_dir, track.id); + std::filesystem::create_directories(dir); + track.save(dir + "/track.json"); +} + +void pauseForEnter(const std::string& message) { + std::cout << message << " (press Enter to continue)"; + std::cout.flush(); + std::cin.get(); +} + +} // namespace + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + FLAGS_minloglevel = 0; + + CLI::App app("Replay an entire saved run through a fresh MaxIoUTracker for parameter tuning"); + + std::string run_dir; + std::string output_dir; + int verbosity = 0; + // NOTE: these default to the DEPLOYED config's values (dcist_launch_system/config/default_awcd/ + // hydra.yaml), not MaxIoUTracker::Config's own in-code defaults, so a bare invocation with no + // flags reproduces production out of the box. (Previously defaulted to the code defaults -- + // 0.5/0.5/20/disabled -- which are all stricter than production and silently produced ~2x the + // real fragmentation; see tracker_debug.md's usability-fix writeup.) Still fully overridable. + float min_semantic_iou = 0.25f; + float min_cosine_sim = 0.0f; + float min_cross_iou = 0.1f; + int min_num_observations = 10; + std::string bbox_type = "aabb"; + std::string track_by = "pixels"; + std::string map_window_type = "spatial"; + double map_window_radius = 14.0; + double map_window_seconds = 3.0; + bool pause_on_event = false; + bool save_overlay = false; + float min_range = 0.05f; + float max_range = 10.0f; + + app.add_option("--run-dir", run_dir, "Run directory (has camera_intrinsics.json + observations/)") + ->required() + ->check(CLI::ExistingDirectory); + app.add_option("--output-dir", output_dir, "Where to write tracks/ and archived_tracks/") + ->required(); + app.add_option("--verbosity", verbosity, + "Tracker verbosity; >=6 prints per-cluster accept/reject reasons") + ->default_val(0); + app.add_option("--min-semantic-iou", min_semantic_iou, + "MaxIoUTracker::Config::min_semantic_iou (default matches the deployed config)") + ->default_val(0.25f); + app.add_option("--min-cosine-sim", min_cosine_sim, "MaxIoUTracker::Config::min_cosine_sim") + ->default_val(0.0f); + app.add_option("--min-cross-iou", min_cross_iou, + "MaxIoUTracker::Config::min_cross_iou (default matches the deployed config)") + ->default_val(0.1f); + app.add_option("--min-num-observations", min_num_observations, + "MaxIoUTracker::Config::min_num_observations (default matches the deployed config)") + ->default_val(10); + app.add_option("--bbox-type", bbox_type, "MaxIoUTracker::Config::bbox_type (aabb|raabb)") + ->default_val("aabb"); + app.add_option("--track-by", track_by, "MaxIoUTracker::Config::track_by (pixels|voxels|bounding_box)") + ->default_val("pixels"); + app.add_option("--map-window-type", map_window_type, + "Active-window eviction policy (spatial|temporal), matches hydra's map_window config") + ->default_val("spatial"); + app.add_option("--map-window-radius", map_window_radius, + "SpatialWindowChecker::Config::max_radius_m [m]; used if --map-window-type=spatial") + ->default_val(14.0); + app.add_option("--min-range", min_range, + "Minimum depth [m] for a pixel to be included in a cluster's pixels; mirrors " + "instance_forwarding.cpp's own pixel-validity gate. Default matches the deployed " + "config; pass 0 to disable. Non-finite depth is always excluded regardless.") + ->default_val(0.05f); + app.add_option("--max-range", max_range, + "Maximum depth [m] for a pixel to be included; mirrors instance_forwarding.cpp. " + "Default matches the deployed config; pass 0 to disable.") + ->default_val(10.0f); + app.add_option("--map-window-seconds", map_window_seconds, + "TemporalWindowChecker::Config::window_sec [s]; used if --map-window-type=temporal") + ->default_val(3.0); + app.add_flag("--pause-on-event", pause_on_event, + "Block on stdin (press Enter) only at frames where a track was created or evicted, " + "instead of every frame."); + app.add_flag("--save-overlay", save_overlay, + "Save overlay_.png (color + red mask blend) into each track's output directory " + "for every frame it is updated, for offline visualization. Off by default -- adds " + "one image write per track per frame, which can be significant over a full run."); + + CLI11_PARSE(app, argc, argv); + + const auto camera = loadCameraIntrinsics(run_dir); + if (!camera) { + return EXIT_FAILURE; + } + + MaxIoUTracker::Config tracker_config; + tracker_config.verbosity = verbosity; + tracker_config.min_semantic_iou = min_semantic_iou; + tracker_config.min_cosine_sim = min_cosine_sim; + tracker_config.min_cross_iou = min_cross_iou; + tracker_config.min_num_observations = min_num_observations; + tracker_config.bbox_type = bbox_type == "raabb" ? MaxIoUTracker::Config::BBoxType::kRAABB + : MaxIoUTracker::Config::BBoxType::kAABB; + if (track_by == "voxels") { + tracker_config.track_by = MaxIoUTracker::Config::TrackBy::kVoxels; + } else if (track_by == "bounding_box") { + tracker_config.track_by = MaxIoUTracker::Config::TrackBy::kBouningBox; + } else { + tracker_config.track_by = MaxIoUTracker::Config::TrackBy::kPixels; + } + MaxIoUTracker tracker(tracker_config); + + std::unique_ptr map_window; + if (map_window_type == "temporal") { + hydra::TemporalWindowChecker::Config cfg; + cfg.window_sec = map_window_seconds; + map_window = std::make_unique(cfg); + } else { + hydra::SpatialWindowChecker::Config cfg; + cfg.max_radius_m = map_window_radius; + map_window = std::make_unique(cfg); + } + + const auto stamps = sortedObservationStamps(run_dir); + std::cout << "Replaying " << stamps.size() << " observations from " << run_dir << "\n"; + + Tracks tracks; + size_t total_tracks_created = 0; + size_t total_tracks_archived = 0; + + for (const TimeStamp stamp : stamps) { + const auto frame_data = FrameData::load( + run_dir + "/observations/" + std::to_string(stamp), camera, stamp, min_range, max_range); + if (!frame_data) { + std::cerr << "Failed to reconstruct FrameData for stamp " << stamp << "; skipping.\n"; + continue; + } + + const auto ids_before = trackIds(tracks); + tracker.processInput(*frame_data, tracks); + + std::vector new_ids; + for (const auto& track : tracks) { + if (!ids_before.count(track.id)) { + new_ids.push_back(track.id); + } + } + total_tracks_created += new_ids.size(); + + if (save_overlay) { + for (const auto& track : tracks) { + if (track.last_seen == stamp) { + saveOverlay(output_dir, track, *frame_data); + } + } + } + + // Mirrors ActiveWindow::updateTrackingStatus: recompute is_active for every track using the + // real eviction policy, keyed off this frame's robot pose. + for (auto& track : tracks) { + const Eigen::Vector3d track_pos = track.last_bounding_box.world_P_center.cast(); + track.is_active = map_window->inBounds( + stamp, frame_data->input.world_T_body, track.last_seen, track_pos); + } + + // Mirrors ActiveWindow::extractInactiveObjects: erase and archive newly-inactive tracks. + std::vector evicted_ids; + auto it = tracks.begin(); + while (it != tracks.end()) { + if (it->is_active) { + ++it; + continue; + } + evicted_ids.push_back(it->id); + archiveTrack(output_dir, *it); + ++total_tracks_archived; + it = tracks.erase(it); + } + + if (pause_on_event && (!new_ids.empty() || !evicted_ids.empty())) { + std::cout << "\n[stamp " << stamp << "] "; + if (!new_ids.empty()) { + std::cout << "NEW TRACK(S): "; + for (int id : new_ids) { + std::cout << id << " "; + } + } + if (!evicted_ids.empty()) { + std::cout << "EVICTED: "; + for (int id : evicted_ids) { + std::cout << id << " "; + } + } + std::cout << "| tracks currently active: " << tracks.size(); + pauseForEnter(""); + } + } + + for (const auto& track : tracks) { + saveActiveTrack(output_dir, track); + } + + std::cout << "\n=== Summary ===\n"; + std::cout << "Frames processed: " << stamps.size() << "\n"; + std::cout << "Total tracks created: " << total_tracks_created << "\n"; + std::cout << "Active at end: " << tracks.size() << "\n"; + std::cout << "Archived (evicted) during run: " << total_tracks_archived << "\n"; + std::cout << "Results written to: " << output_dir << " (tracks/ + archived_tracks/)\n"; + + return EXIT_SUCCESS; +} diff --git a/khronos/app/test_frame_data_serialization.cpp b/khronos/app/test_frame_data_serialization.cpp new file mode 100644 index 0000000..c10b074 --- /dev/null +++ b/khronos/app/test_frame_data_serialization.cpp @@ -0,0 +1,221 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +// Standalone round-trip check for FrameData::save/FrameData::load and saveCameraIntrinsics/ +// loadCameraIntrinsics (khronos/active_window/data/frame_data.h). Builds a synthetic FrameData +// with one semantic and one dynamic cluster (distinct pixel regions, category_id, and feature +// vectors), saves it plus the camera intrinsics, reloads both, and compares field-by-field. +// Exits non-zero and prints a diagnostic on the first mismatch. This validates Step 3 of the +// tracker-debugging pipeline (see tracker_debug.md) -- the full per-frame segmentation format +// (object_image/dynamic_image + clusters.json) that resolves Step 2's "no per-frame semantics" +// caveat. + +#include +#include +#include +#include + +#include +#include + +#include "khronos/active_window/data/frame_data.h" + +namespace { + +using khronos::FeatureVector; +using khronos::FrameData; +using khronos::MeasurementCluster; +using khronos::Pixel; +using khronos::SemanticClusterInfo; +using khronos::TimeStamp; + +constexpr int kWidth = 8; +constexpr int kHeight = 6; +constexpr int kSemanticId = 5; +constexpr int kDynamicId = 9; +constexpr TimeStamp kStamp = 123456789; + +std::shared_ptr makeCamera() { + hydra::Camera::Config config; + config.min_range = 0.01; + config.max_range = 100.0; + config.fx = 50.5f; + config.fy = 51.5f; + config.cx = 4.0f; + config.cy = 3.0f; + config.width = kWidth; + config.height = kHeight; + config.extrinsics = hydra::IdentitySensorExtrinsics::Config{}; + return std::make_shared(config, "test_camera"); +} + +FrameData::Ptr makeSampleFrameData(const std::shared_ptr& camera) { + hydra::InputData input(camera); + input.timestamp_ns = kStamp; + input.world_T_body = Eigen::Translation3d(1.0, 2.0, 0.5) * Eigen::Quaterniond::Identity(); + + input.color_image = cv::Mat(kHeight, kWidth, CV_8UC3, cv::Scalar(10, 20, 30)); + input.depth_image = cv::Mat(kHeight, kWidth, CV_32FC1, cv::Scalar(2.0f)); + + auto frame_data = std::make_shared(input); + + frame_data->object_image = cv::Mat::zeros(kHeight, kWidth, CV_32SC1); + frame_data->object_image(cv::Rect(0, 0, 2, 2)).setTo(kSemanticId); + + frame_data->dynamic_image = cv::Mat::zeros(kHeight, kWidth, CV_32SC1); + frame_data->dynamic_image(cv::Rect(4, 3, 2, 2)).setTo(kDynamicId); + + MeasurementCluster semantic_cluster; + semantic_cluster.id = kSemanticId; + // Must match the object_image region set above (Rect(0, 0, 2, 2)) so the round-trip comparison + // (original pixels vs. pixels reconstructed by scanning object_image) is apples-to-apples. + semantic_cluster.pixels = {Pixel(0, 0), Pixel(1, 0), Pixel(0, 1), Pixel(1, 1)}; + semantic_cluster.bounding_box = + spark_dsg::BoundingBox(Eigen::Vector3f(1.0f, 1.0f, 1.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f)); + FeatureVector feature(3); + feature << 0.5f, 0.25f, 0.125f; + semantic_cluster.semantics = SemanticClusterInfo(3, feature); + frame_data->semantic_clusters = {semantic_cluster}; + + MeasurementCluster dynamic_cluster; + dynamic_cluster.id = kDynamicId; + // Must match the dynamic_image region set above (Rect(4, 3, 2, 2)). + dynamic_cluster.pixels = {Pixel(4, 3), Pixel(5, 3), Pixel(4, 4), Pixel(5, 4)}; + dynamic_cluster.bounding_box = + spark_dsg::BoundingBox(Eigen::Vector3f(0.5f, 0.5f, 0.5f), Eigen::Vector3f(1.0f, 1.0f, 0.0f)); + dynamic_cluster.semantics = SemanticClusterInfo(-1); // no openset feature. + frame_data->dynamic_clusters = {dynamic_cluster}; + + return frame_data; +} + +bool checkEqual(bool condition, const std::string& what) { + if (!condition) { + std::cerr << "MISMATCH: " << what << std::endl; + } + return condition; +} + +bool pixelsMatch(const khronos::Pixels& lhs, const khronos::Pixels& rhs) { + if (lhs.size() != rhs.size()) { + return false; + } + std::set> lhs_set; + std::set> rhs_set; + for (const Pixel& p : lhs) { + lhs_set.emplace(p.u, p.v); + } + for (const Pixel& p : rhs) { + rhs_set.emplace(p.u, p.v); + } + return lhs_set == rhs_set; +} + +bool clusterMatch(const MeasurementCluster& original, const MeasurementCluster& reloaded, + const std::string& label) { + bool ok = true; + ok &= checkEqual(original.id == reloaded.id, label + ".id"); + ok &= checkEqual(pixelsMatch(original.pixels, reloaded.pixels), label + ".pixels"); + ok &= checkEqual(original.bounding_box == reloaded.bounding_box, label + ".bounding_box"); + ok &= checkEqual(original.semantics.has_value() == reloaded.semantics.has_value(), + label + ".semantics.has_value"); + if (ok && original.semantics) { + ok &= checkEqual(original.semantics->category_id == reloaded.semantics->category_id, + label + ".semantics.category_id"); + ok &= checkEqual((original.semantics->feature - reloaded.semantics->feature).norm() < 1e-5f, + label + ".semantics.feature"); + } + return ok; +} + +} // namespace + +int main() { + const auto tmp_dir = std::filesystem::temp_directory_path() / "khronos_frame_data_roundtrip_test"; + std::filesystem::remove_all(tmp_dir); + std::filesystem::create_directories(tmp_dir); + const auto observation_dir = tmp_dir / "observations" / std::to_string(kStamp); + + const auto camera = makeCamera(); + const auto original = makeSampleFrameData(camera); + + original->save(observation_dir.string()); + khronos::saveCameraIntrinsics(*camera, tmp_dir.string()); + + const auto reloaded_camera = khronos::loadCameraIntrinsics(tmp_dir.string()); + const auto reloaded = FrameData::load(observation_dir.string(), reloaded_camera, kStamp); + + std::filesystem::remove_all(tmp_dir); + + if (!reloaded_camera) { + std::cerr << "FrameData round-trip FAILED: loadCameraIntrinsics returned nullptr." << std::endl; + return EXIT_FAILURE; + } + if (!reloaded) { + std::cerr << "FrameData round-trip FAILED: FrameData::load returned nullptr." << std::endl; + return EXIT_FAILURE; + } + + bool ok = true; + const auto& original_cfg = camera->getConfig(); + const auto& reloaded_cfg = reloaded_camera->getConfig(); + ok &= checkEqual(std::abs(original_cfg.fx - reloaded_cfg.fx) < 1e-3f, "camera.fx"); + ok &= checkEqual(std::abs(original_cfg.fy - reloaded_cfg.fy) < 1e-3f, "camera.fy"); + ok &= checkEqual(std::abs(original_cfg.cx - reloaded_cfg.cx) < 1e-3f, "camera.cx"); + ok &= checkEqual(std::abs(original_cfg.cy - reloaded_cfg.cy) < 1e-3f, "camera.cy"); + ok &= checkEqual(original_cfg.width == reloaded_cfg.width, "camera.width"); + ok &= checkEqual(original_cfg.height == reloaded_cfg.height, "camera.height"); + + ok &= checkEqual(reloaded->input.timestamp_ns == kStamp, "input.timestamp_ns"); + ok &= checkEqual(reloaded->semantic_clusters.size() == 1, "semantic_clusters.size"); + ok &= checkEqual(reloaded->dynamic_clusters.size() == 1, "dynamic_clusters.size"); + + if (ok && reloaded->semantic_clusters.size() == 1) { + ok &= clusterMatch(original->semantic_clusters[0], reloaded->semantic_clusters[0], "semantic"); + } + if (ok && reloaded->dynamic_clusters.size() == 1) { + ok &= clusterMatch(original->dynamic_clusters[0], reloaded->dynamic_clusters[0], "dynamic"); + } + + if (!ok) { + std::cerr << "FrameData round-trip FAILED." << std::endl; + return EXIT_FAILURE; + } + + std::cout << "FrameData round-trip OK: all fields match." << std::endl; + return EXIT_SUCCESS; +} diff --git a/khronos/app/test_track_association.cpp b/khronos/app/test_track_association.cpp new file mode 100644 index 0000000..b3a344b --- /dev/null +++ b/khronos/app/test_track_association.cpp @@ -0,0 +1,536 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +/** + * Step 2 of the tracker-debugging pipeline (see tracker_debug.md): replays one track's saved + * observations through the live MaxIoUTracker against another already-loaded track, to observe + * directly whether/why the tracker associates them or spawns a new track. See + * khronos/active_window/data/frame_data.h (FrameData::save/load) for the on-disk format this + * reads -- Step 3 formalized that as a full FrameData serialization (color/depth/pose + + * object_image/dynamic_image + clusters.json with real per-cluster semantics), so cluster + * semantics used here are the actual per-frame values, not an aggregated-Track approximation. + * + * Inputs are `tracks/track_/` directories written by ActiveWindowTrackSaver (see + * track_saver_sink.cpp), each containing track.json plus symlinks back to the run's shared + * `observations//` folders (color/depth/pose/segmentation, one per real frame). + * + * ------------------------------------------------------------------------------------------- + * USAGE + * ------------------------------------------------------------------------------------------- + * + * Binary: install/khronos/bin/test_track_association (after `colcon build --packages-select + * khronos`). Run with --help to see all flags. + * + * IMPORTANT: pass the actual tracker config the run used (base_params/hydra.yaml or an + * experiment override, e.g. dcist_launch_system/config/default_awcd/hydra.yaml's + * `active_window.tracker` block), not the app's built-in MaxIoUTracker::Config defaults -- they + * can differ (e.g. this codebase's default_awcd config uses min_semantic_iou: 0.25, not the + * Config default of 0.5) and change whether a given IoU passes or fails. + * + * Mode 1 -- compare two different saved tracks (e.g. "did track_0 and track_3 fail to merge, + * and why?"): + * + * test_track_association \ + * --existing-track-dir /tracks/track_0 \ + * --observation-track-dir /tracks/track_3 \ + * --bbox-type aabb --min-semantic-iou 0.25 --min-cross-iou 0.1 --min-num-observations 10 \ + * --verbosity 6 + * + * - Replays every observation in --observation-track-dir (in chronological order) against the + * track loaded from --existing-track-dir. The run directory (containing camera_intrinsics.json + * and observations/) is derived automatically from each track dir's grandparent. + * - The existing track's last_points/last_bounding_box are auto-derived from its own historical + * observation immediately preceding --observation-track-dir's earliest observation (rather + * than its FINAL saved snapshot, which -- since Track only ever persists one, see Track::save + * -- may be far in time/viewpoint from the frames under test and would show artificially low + * IoU). No manual stamp-hunting needed. Pass --existing-track-stamp explicitly to override + * this auto-derivation with a different historical decision point. + * - --verbosity 6 surfaces MaxIoUTracker's own CLOG(6) accept/reject lines (low IoU vs. + * semantic mismatch, etc.) so you can see exactly why a cluster was or wasn't associated. + * - Per-frame RESULT lines report NEW TRACK CREATED / ASSOCIATED / dropped silently; a + * WARNING is printed if a newly-created track's id collides with the existing track's id (a + * fresh MaxIoUTracker's internal id counter starts at 0, same as many loaded tracks -- the + * harness tracks the "existing" track by its stable vector position, index 0, not by + * Track::id). + * + * Mode 2 -- self-consistency sanity check ("does the harness reproduce a track as ONE + * continuous track when replayed from scratch, matching what production actually did?"): + * + * test_track_association \ + * --from-scratch \ + * --observation-track-dir /tracks/track_0 \ + * --bbox-type aabb --min-semantic-iou 0.25 --min-cross-iou 0.1 --min-num-observations 10 + * + * - Omit --existing-track-dir; starts with an empty track list and replays + * --observation-track-dir's own observation history in order, letting the tracker create + * and grow the track exactly as production's incremental processInput() calls would. + * - Ends with a summary: "PASS: stayed as ONE continuous track" if it never split, or "FAIL: + * fragmented into N tracks" with each fragment's id/observation count/time range if it did. + * - Useful to validate the harness itself isn't the source of a reported fragmentation before + * trusting a Mode 1 result. + * + * Both modes accept --save-reprojection-dir to additionally save, per replayed frame, + * reprojection_.png -- the frame's real detection cluster in semi-transparent red overlaid + * with the candidate track's (tracks[0]) reprojected last_points in semi-transparent green; + * overlapping pixels are blended twice and read as a yellow-ish tone (only meaningful for the + * default --track-by pixels; unset by default, so no files are written unless requested). + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "khronos/active_window/data/frame_data.h" +#include "khronos/active_window/data/track.h" +#include "khronos/active_window/tracking/max_iou_tracker.h" + +using namespace khronos; + +namespace { + +std::vector sortedObservationStamps(const Track& track) { + std::vector stamps; + stamps.reserve(track.observations.size()); + for (const auto& obs : track.observations) { + stamps.push_back(obs.stamp); + } + std::sort(stamps.begin(), stamps.end()); + return stamps; +} + +// track_dir is always /tracks/track_ (see ActiveWindowTrackSaver); the run root +// (containing camera_intrinsics.json and observations/) is its grandparent. +std::string runDirFromTrackDir(const std::string& track_dir) { + return std::filesystem::path(track_dir).parent_path().parent_path().string(); +} + +FrameData::Ptr loadObservation(const std::string& run_dir, + const std::shared_ptr& camera, + TimeStamp stamp) { + return FrameData::load(run_dir + "/observations/" + std::to_string(stamp), camera, stamp); +} + +// Finds the (id, is_dynamic) of the cluster `track` was associated with at the given stamp, per +// its own saved observations[] list. +std::optional> clusterIdAtStamp(const Track& track, TimeStamp stamp) { + for (const auto& obs : track.observations) { + if (obs.stamp != stamp) { + continue; + } + if (obs.semantic_cluster_id >= 0) { + return std::make_pair(obs.semantic_cluster_id, false); + } + if (obs.dynamic_cluster_id >= 0) { + return std::make_pair(obs.dynamic_cluster_id, true); + } + return std::nullopt; + } + return std::nullopt; +} + +// Finds the latest of `track`'s own observation stamps that is <= `before_stamp`. Used to +// auto-derive --existing-track-stamp: when comparing whether `observation_track` (e.g. track_3) +// should have associated into `track` (e.g. track_0), the relevant historical decision point is +// track_0's own state at its last observation before track_3 first appeared. +std::optional latestStampBefore(const Track& track, TimeStamp before_stamp) { + std::optional best; + for (const auto& obs : track.observations) { + if (obs.stamp <= before_stamp && (!best || obs.stamp > *best)) { + best = obs.stamp; + } + } + return best; +} + +// Removes every cluster from `frame_data` except the one with the given (id, is_dynamic) -- +// preserves this harness's "does this one specific detection associate" semantics now that +// FrameData::load reconstructs the full multi-object frame. (Full multi-cluster competitive +// replay across the whole frame is Step 4's job, not this harness's.) +void keepOnlyCluster(FrameData* frame_data, int id, bool is_dynamic) { + auto& keep = is_dynamic ? frame_data->dynamic_clusters : frame_data->semantic_clusters; + auto& clear = is_dynamic ? frame_data->semantic_clusters : frame_data->dynamic_clusters; + clear.clear(); + keep.erase(std::remove_if(keep.begin(), + keep.end(), + [id](const MeasurementCluster& c) { return c.id != id; }), + keep.end()); +} + +// Track only ever persists its FINAL last_points/last_bounding_box snapshot (see +// Track::save/load), not a per-observation history. Replaying against that final snapshot +// reprojects points from whatever the track's very last observation was -- if that is far in time +// (and thus viewpoint) from the frame under test, computeIoUPixels will show low IoU purely from +// that staleness, regardless of whether the two detections are the same physical object. This +// rebuilds `track.last_points`/`last_bounding_box`/`semantics` as they actually were at a specific +// earlier on-disk observation of `track_dir` (using the REAL per-frame semantics from +// clusters.json, not the track's aggregated approximation), so replay can test the tracker's real +// decision point instead of an artifact of final-state persistence. +bool overrideWithHistoricalObservation(const std::string& track_dir, TimeStamp stamp, Track* track) { + const std::string run_dir = runDirFromTrackDir(track_dir); + const auto camera = loadCameraIntrinsics(run_dir); + if (!camera) { + return false; + } + + const auto cluster_ref = clusterIdAtStamp(*track, stamp); + if (!cluster_ref) { + std::cerr << "Track has no observation at stamp " << stamp << " in " << track_dir << "\n"; + return false; + } + const auto [cluster_id, is_dynamic] = *cluster_ref; + + const auto frame_data = loadObservation(run_dir, camera, stamp); + if (!frame_data) { + std::cerr << "Failed to reconstruct historical observation at stamp " << stamp << " in " + << track_dir << "\n"; + return false; + } + + const auto& clusters = is_dynamic ? frame_data->dynamic_clusters : frame_data->semantic_clusters; + const auto it = std::find_if(clusters.begin(), clusters.end(), [cluster_id](const auto& c) { + return c.id == cluster_id; + }); + if (it == clusters.end()) { + std::cerr << "Cluster id " << cluster_id << " not found in observation at stamp " << stamp + << "\n"; + return false; + } + + Points last_points; + last_points.reserve(it->pixels.size()); + for (const Pixel& pixel : it->pixels) { + const auto& point = frame_data->input.vertex_map.at(pixel.v, pixel.u); + last_points.emplace_back(point[0], point[1], point[2]); + } + + track->last_points = std::move(last_points); + track->last_bounding_box = it->bounding_box; + track->semantics = it->semantics; // real per-frame semantics, not the aggregated approximation. + track->last_seen = stamp; + return true; +} + +// Blends `color` into `image` at `alpha` opacity, restricted to `mask` (255-valued pixels). +void alphaBlendMask(cv::Mat* image, const cv::Mat& mask, const cv::Scalar& color, double alpha) { + cv::Mat color_layer(image->size(), image->type(), color); + cv::Mat blended; + cv::addWeighted(*image, 1.0 - alpha, color_layer, alpha, 0, blended); + blended.copyTo(*image, mask); +} + +// Saves a dual-color comparison image: cluster's real pixels in semi-transparent red, the track's +// reprojected points (see MaxIoUTracker::reprojectPoints) in semi-transparent green -- each mask +// is alpha-blended into the image independently and restricted to its own region, so a pixel hit +// by both masks is blended twice (red then green) and naturally reads as a blended yellow-ish +// tone, while background pixels stay untouched (full brightness). +void saveReprojectionOverlay(const std::string& dir, + TimeStamp stamp, + const FrameData& frame_data, + const MeasurementCluster& cluster, + const std::set& reprojected_pixels, + float iou) { + if (frame_data.input.color_image.empty()) { + return; + } + std::filesystem::create_directories(dir); + + cv::Mat bgr_image; + cv::cvtColor(frame_data.input.color_image, bgr_image, cv::COLOR_RGB2BGR); + + cv::Mat detection_mask = cv::Mat::zeros(bgr_image.size(), CV_8UC1); + for (const Pixel& pixel : cluster.pixels) { + if (pixel.isInImage(detection_mask)) { + detection_mask.at(pixel.v, pixel.u) = 255; + } + } + cv::Mat reprojected_mask = cv::Mat::zeros(bgr_image.size(), CV_8UC1); + for (const Pixel& pixel : reprojected_pixels) { + if (pixel.isInImage(reprojected_mask)) { + reprojected_mask.at(pixel.v, pixel.u) = 255; + } + } + + cv::Mat blended = bgr_image.clone(); + alphaBlendMask(&blended, detection_mask, cv::Scalar(0, 0, 255), 0.4); // Red: real detection. + alphaBlendMask(&blended, reprojected_mask, cv::Scalar(0, 255, 0), 0.4); // Green: reprojected track. + + std::ostringstream iou_str; + iou_str << std::fixed << std::setprecision(3) << iou; + cv::imwrite( + dir + "/reprojection_" + std::to_string(stamp) + "_iou_" + iou_str.str() + ".png", blended); +} + +} // namespace + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = true; + FLAGS_minloglevel = 0; + + CLI::App app("Replay an observation track through MaxIoUTracker against an existing track"); + + std::string existing_track_dir; + std::string observation_track_dir; + int verbosity = 6; + float min_semantic_iou = 0.25f; + float min_cosine_sim = 0.0f; + float min_cross_iou = 0.1f; + int min_num_observations = 10; + std::string bbox_type = "aabb"; + long long existing_track_stamp = -1; + std::string save_reprojection_dir; + + bool from_scratch = false; + + app.add_option("--existing-track-dir", existing_track_dir, + "Directory (e.g. .../track_0) of the track to treat as already-tracked. Omit " + "with --from-scratch to instead replay --observation-track-dir's own history " + "starting from an empty track list (self-consistency sanity check).") + ->check(CLI::ExistingDirectory); + app.add_flag("--from-scratch", from_scratch, + "Ignore --existing-track-dir; start with no tracks and replay " + "--observation-track-dir's own observation history from scratch, to check whether " + "the harness reproduces it as a single continuous track (sanity check that the " + "harness matches production behavior, not a test of fragmentation against a " + "*different* track)."); + app.add_option("--observation-track-dir", observation_track_dir, + "Directory (e.g. .../track_3) whose observations are replayed as new detections") + ->required() + ->check(CLI::ExistingDirectory); + app.add_option("--verbosity", verbosity, + "Tracker verbosity; >=6 prints per-cluster accept/reject reasons") + ->default_val(6); + app.add_option("--min-semantic-iou", min_semantic_iou, "MaxIoUTracker::Config::min_semantic_iou") + ->default_val(0.5f); + app.add_option("--min-cosine-sim", min_cosine_sim, "MaxIoUTracker::Config::min_cosine_sim") + ->default_val(0.0f); + app.add_option("--min-cross-iou", min_cross_iou, "MaxIoUTracker::Config::min_cross_iou") + ->default_val(0.5f); + app.add_option( + "--min-num-observations", min_num_observations, "MaxIoUTracker::Config::min_num_observations") + ->default_val(20); + app.add_option("--bbox-type", bbox_type, "MaxIoUTracker::Config::bbox_type (aabb|raabb)") + ->default_val("aabb"); + app.add_option("--existing-track-stamp", existing_track_stamp, + "Optional: override the existing track's last_points/last_bounding_box using its " + "OWN on-disk observation at this stamp (ns), instead of its final saved snapshot. " + "If omitted, this is auto-derived as the existing track's latest observation stamp " + "at or before --observation-track-dir's earliest observation stamp -- pass this " + "flag explicitly only to override that choice."); + app.add_option("--save-reprojection-dir", save_reprojection_dir, + "Optional: directory to save per-frame reprojection comparison images " + "(reprojection_.png) -- the seed/candidate track (tracks[0])'s reprojected " + "last_points in blue, overlaid with the real detection cluster in green. Only " + "meaningful when --track-by is 'pixels' (the default) and a candidate track with " + "non-empty last_points exists; unset by default (no files written). In " + "--from-scratch mode with fragmentation, this only visualizes against tracks[0]."); + + CLI11_PARSE(app, argc, argv); + + if (!from_scratch && existing_track_dir.empty()) { + std::cerr << "Either --existing-track-dir or --from-scratch is required.\n"; + return EXIT_FAILURE; + } + + const Track observation_track = Track::load(observation_track_dir + "/track.json"); + std::cout << "Loaded observation track id=" << observation_track.id + << " (" << observation_track.observations.size() << " observations, " + << observation_track.first_seen << " -> " << observation_track.last_seen << ")\n"; + + Tracks tracks; + if (!from_scratch) { + Track existing_on_disk = Track::load(existing_track_dir + "/track.json"); + if (existing_track_stamp < 0) { + const auto observation_stamps = sortedObservationStamps(observation_track); + if (!observation_stamps.empty()) { + const auto derived = latestStampBefore(existing_on_disk, observation_stamps.front()); + if (derived) { + existing_track_stamp = static_cast(*derived); + std::cout << "Auto-derived --existing-track-stamp " << existing_track_stamp + << " (existing track's latest observation at or before observation track's " + "earliest stamp " << observation_stamps.front() << "). Pass " + "--existing-track-stamp explicitly to override.\n"; + } else { + std::cout << "No auto-derivation possible: existing track has no observation at or " + "before observation track's earliest stamp " << observation_stamps.front() + << "; using existing track's final saved snapshot instead.\n"; + } + } + } + if (existing_track_stamp >= 0) { + if (!overrideWithHistoricalObservation(existing_track_dir, + static_cast(existing_track_stamp), + &existing_on_disk)) { + return EXIT_FAILURE; + } + std::cout << "Overrode existing track's last_points/last_bounding_box using its own " + "observation at stamp " << existing_track_stamp << "\n"; + } + std::cout << "Loaded existing track id=" << existing_on_disk.id + << " (" << existing_on_disk.observations.size() << " observations, " + << existing_on_disk.first_seen << " -> " << existing_on_disk.last_seen << ")\n"; + tracks = {existing_on_disk}; + } else { + std::cout << "Starting from scratch (no seed track); replaying " + << observation_track.observations.size() + << " of the observation track's own observations in order.\n"; + } + + MaxIoUTracker::Config config; + config.verbosity = verbosity; + config.min_semantic_iou = min_semantic_iou; + config.min_cosine_sim = min_cosine_sim; + config.min_cross_iou = min_cross_iou; + config.min_num_observations = min_num_observations; + config.bbox_type = bbox_type == "raabb" ? MaxIoUTracker::Config::BBoxType::kRAABB + : MaxIoUTracker::Config::BBoxType::kAABB; + MaxIoUTracker tracker(config); + + // NOTE: a freshly constructed MaxIoUTracker starts its internal id counter at 0 + // (current_track_id_), which can collide with a loaded existing track's on-disk id (also + // frequently 0). So the "existing"/"seed" track (when present) is identified by its STABLE + // VECTOR POSITION (index 0 -- addNewTrack only ever tracks.emplace_back()s, never inserts + // before or reorders), not by Track::id, which cannot be trusted to stay unique against newly + // minted tracks in this harness. + const bool has_seed_track = !tracks.empty(); + + const std::string observation_run_dir = runDirFromTrackDir(observation_track_dir); + const auto observation_camera = loadCameraIntrinsics(observation_run_dir); + if (!observation_camera) { + return EXIT_FAILURE; + } + + for (const TimeStamp stamp : sortedObservationStamps(observation_track)) { + const auto cluster_ref = clusterIdAtStamp(observation_track, stamp); + if (!cluster_ref) { + std::cerr << "Observation track has no cluster id at stamp " << stamp << "; skipping.\n"; + continue; + } + const auto [cluster_id, is_dynamic] = *cluster_ref; + + const auto frame_data = loadObservation(observation_run_dir, observation_camera, stamp); + if (!frame_data) { + std::cerr << "Failed to reconstruct FrameData for stamp " << stamp << "; skipping.\n"; + continue; + } + // Only test whether THIS specific detection associates with the seed track -- clear every + // other cluster in the frame so the tracker can't associate against unrelated objects. + keepOnlyCluster(frame_data.get(), cluster_id, is_dynamic); + + if (!save_reprojection_dir.empty() && + config.track_by == MaxIoUTracker::Config::TrackBy::kPixels && !tracks.empty() && + !tracks[0].last_points.empty()) { + const auto& kept_clusters = is_dynamic ? frame_data->dynamic_clusters : frame_data->semantic_clusters; + if (!kept_clusters.empty()) { + const auto reprojected_pixels = tracker.reprojectPoints(*frame_data, tracks[0].last_points); + const float iou = + tracker.computeIoUPixels(*frame_data, kept_clusters.front(), tracks[0]); + saveReprojectionOverlay( + save_reprojection_dir, stamp, *frame_data, kept_clusters.front(), reprojected_pixels, iou); + } + } + + const size_t tracks_before = tracks.size(); + std::vector obs_before(tracks.size()); + for (size_t i = 0; i < tracks.size(); ++i) { + obs_before[i] = tracks[i].observations.size(); + } + + tracker.processInput(*frame_data, tracks); + + std::cout << "\n=== Replayed observation stamp " << stamp << " ===\n"; + if (tracks.size() > tracks_before) { + // The very first frame in --from-scratch mode goes 0 -> 1: that is the initial track + // being created, not a fragmentation event. + const bool is_initial_creation = !has_seed_track && tracks_before == 0; + std::cout << "RESULT: " << (is_initial_creation ? "INITIAL TRACK CREATED" : "NEW TRACK " + "CREATED (fragmentation reproduced)") + << ". tracks.size() " << tracks_before << " -> " << tracks.size() << "\n"; + } else { + // tracks.size() unchanged -- find which existing track (by stable vector index, matching + // this file's other id-vs-index conventions, see has_seed_track above) actually grew, so a + // detection that associated into a PREVIOUSLY-created fragment track (not the seed) isn't + // misreported as a silent drop. + std::optional grown_index; + for (size_t i = 0; i < obs_before.size() && i < tracks.size(); ++i) { + if (tracks[i].observations.size() > obs_before[i]) { + grown_index = i; + break; + } + } + if (grown_index && *grown_index == 0 && has_seed_track) { + std::cout << "RESULT: ASSOCIATED into seed track (observations " << obs_before[0] + << " -> " << tracks[0].observations.size() << ")\n"; + } else if (grown_index && has_seed_track) { + std::cout << "RESULT: associated into track index " << *grown_index << " (id=" + << tracks[*grown_index].id << ", NOT the seed track) -- observations " + << obs_before[*grown_index] << " -> " << tracks[*grown_index].observations.size() + << "\n"; + } else if (grown_index) { + std::cout << "RESULT: associated into track index " << *grown_index << " (id=" + << tracks[*grown_index].id << ") -- observations " << obs_before[*grown_index] + << " -> " << tracks[*grown_index].observations.size() << "\n"; + } else { + std::cout << "RESULT: no track count change and no observation added -- detection was " + "dropped silently.\n"; + } + } + } + + std::cout << "\n=== Summary ===\n"; + std::cout << "Total distinct tracks after replay: " << tracks.size() << "\n"; + if (tracks.size() == 1) { + std::cout << "PASS: stayed as ONE continuous track across all replayed observations.\n"; + } else { + std::cout << "FAIL: fragmented into " << tracks.size() << " tracks:\n"; + for (size_t i = 0; i < tracks.size(); ++i) { + std::cout << " [" << i << "] id=" << tracks[i].id << " observations=" + << tracks[i].observations.size() << " first_seen=" << tracks[i].first_seen + << " last_seen=" << tracks[i].last_seen << "\n"; + } + } + + return EXIT_SUCCESS; +} diff --git a/khronos/app/test_track_serialization.cpp b/khronos/app/test_track_serialization.cpp new file mode 100644 index 0000000..524bec7 --- /dev/null +++ b/khronos/app/test_track_serialization.cpp @@ -0,0 +1,152 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +// Standalone round-trip check for Track::save / Track::load (khronos/active_window/data/track.h). +// Builds a Track with representative values for every field the tracker consumes (see +// MaxIoUTracker in max_iou_tracker.cpp), writes it to a temp file, reloads it, and compares +// field-by-field. Exits non-zero and prints a diagnostic on the first mismatch. This is the +// minimal validation for Step 1 of the tracker-debugging pipeline (see tracker_debug.md) and the +// exact input format Step 2's replay harness will consume. + +#include +#include +#include + +#include "khronos/active_window/data/track.h" + +namespace { + +using khronos::GlobalIndex; +using khronos::Observation; +using khronos::Point; +using khronos::SemanticClusterInfo; +using khronos::Track; + +Track makeSampleTrack() { + Track track; + track.id = 42; + track.first_seen = 1000; + track.last_seen = 1066; + track.is_dynamic = false; + track.confidence = 0.75f; + track.num_features = 3; + + track.observations = {Observation(1000, 5, -1), Observation(1033, 6, -1), Observation(1066, 7, -1)}; + + track.last_points = {Point(1.0f, 2.0f, 3.0f), Point(4.0f, 5.0f, 6.0f), Point(-1.5f, 0.0f, 2.25f)}; + + track.last_voxels = {GlobalIndex(1, 2, 3), GlobalIndex(-4, 5, -6), GlobalIndex(0, 0, 0)}; + track.last_voxel_size = 0.1f; + + track.last_centroid = Point(1.1f, 2.2f, 3.3f); + + track.last_bounding_box = + spark_dsg::BoundingBox(Eigen::Vector3f(2.0f, 3.0f, 1.0f), Eigen::Vector3f(0.5f, 0.5f, 0.5f)); + + khronos::FeatureVector feature(4); + feature << 0.1f, 0.2f, 0.3f, 0.4f; + track.semantics = SemanticClusterInfo(7, feature); + + return track; +} + +bool checkEqual(bool condition, const std::string& what) { + if (!condition) { + std::cerr << "MISMATCH: " << what << std::endl; + } + return condition; +} + +bool tracksMatch(const Track& original, const Track& reloaded) { + bool ok = true; + ok &= checkEqual(original.id == reloaded.id, "id"); + ok &= checkEqual(original.first_seen == reloaded.first_seen, "first_seen"); + ok &= checkEqual(original.last_seen == reloaded.last_seen, "last_seen"); + ok &= checkEqual(original.is_dynamic == reloaded.is_dynamic, "is_dynamic"); + ok &= checkEqual(std::abs(original.confidence - reloaded.confidence) < 1e-6f, "confidence"); + ok &= checkEqual(original.num_features == reloaded.num_features, "num_features"); + + ok &= checkEqual(original.observations.size() == reloaded.observations.size(), "observations.size"); + for (size_t i = 0; ok && i < original.observations.size(); ++i) { + const auto& lhs = original.observations[i]; + const auto& rhs = reloaded.observations[i]; + ok &= checkEqual(lhs.stamp == rhs.stamp && lhs.semantic_cluster_id == rhs.semantic_cluster_id && + lhs.dynamic_cluster_id == rhs.dynamic_cluster_id, + "observations[" + std::to_string(i) + "]"); + } + + ok &= checkEqual(original.last_points.size() == reloaded.last_points.size(), "last_points.size"); + for (size_t i = 0; ok && i < original.last_points.size(); ++i) { + ok &= checkEqual((original.last_points[i] - reloaded.last_points[i]).norm() < 1e-5f, + "last_points[" + std::to_string(i) + "]"); + } + + ok &= checkEqual(original.last_voxels == reloaded.last_voxels, "last_voxels"); + ok &= checkEqual(std::abs(original.last_voxel_size - reloaded.last_voxel_size) < 1e-6f, + "last_voxel_size"); + ok &= checkEqual((original.last_centroid - reloaded.last_centroid).norm() < 1e-5f, "last_centroid"); + ok &= checkEqual(original.last_bounding_box == reloaded.last_bounding_box, "last_bounding_box"); + + ok &= checkEqual(original.semantics.has_value() == reloaded.semantics.has_value(), "semantics.has_value"); + if (ok && original.semantics) { + ok &= checkEqual(original.semantics->category_id == reloaded.semantics->category_id, + "semantics.category_id"); + ok &= checkEqual((original.semantics->feature - reloaded.semantics->feature).norm() < 1e-5f, + "semantics.feature"); + } + + return ok; +} + +} // namespace + +int main() { + const auto tmp_path = std::filesystem::temp_directory_path() / "khronos_track_roundtrip_test.json"; + + const Track original = makeSampleTrack(); + original.save(tmp_path.string()); + const Track reloaded = Track::load(tmp_path.string()); + std::filesystem::remove(tmp_path); + + if (!tracksMatch(original, reloaded)) { + std::cerr << "Track round-trip FAILED." << std::endl; + return EXIT_FAILURE; + } + + std::cout << "Track round-trip OK: all fields match." << std::endl; + return EXIT_SUCCESS; +} diff --git a/khronos/cmake/KhronosInstall.cmake b/khronos/cmake/KhronosInstall.cmake index 0c9e5df..88f7c02 100644 --- a/khronos/cmake/KhronosInstall.cmake +++ b/khronos/cmake/KhronosInstall.cmake @@ -1,5 +1,5 @@ install( - TARGETS ${PROJECT_NAME} + TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_registration EXPORT khronos-targets LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/khronos/include/khronos/active_window/change_detection/active_window_change_detector.h b/khronos/include/khronos/active_window/change_detection/active_window_change_detector.h new file mode 100644 index 0000000..8b51f45 --- /dev/null +++ b/khronos/include/khronos/active_window/change_detection/active_window_change_detector.h @@ -0,0 +1,322 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "khronos/active_window/active_window.h" +#include "khronos/active_window/change_detection/transformation_getter.h" +#include "khronos/utils/icp_registration_utils.h" + +namespace khronos { + +class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { + public: + /** + * @brief Per-object temporal state for the removed-object EMA filter. + * Maintained across frames in removed_object_states_. + */ + struct RemovedObjectState { + //! EMA-filtered probability that the object's space is free (i.e. object is removed). + double free_probability = 0.0; + //! Number of frames for which a valid measurement (num_known > 0) was available. + int num_frames_observed = 0; + //! Raw free ratio from the most recent valid measurement (for logging/debugging). + float last_free_ratio = 0.0f; + //! Sensor frame time (ns) the object first crossed the removal gate. Latched once set; + //! never reset even if the object later drops out of the removed set and returns. + //! 0 = not yet declared removed. + TimeStamp first_removed_ns = 0; + }; + + /** + * @brief A removed object paired with the sensor frame time it was first declared removed. + * The timestamp is latched at the detector (see RemovedObjectState::first_removed_ns) so it + * stays constant across every message/frame that reports this object as removed. + */ + struct RemovedObject { + spark_dsg::NodeId id; + TimeStamp first_removed_ns; + }; + + /** + * @brief Per-track temporal state for the newly-added-object EMA filter. + * Keyed by track ID (monotonically increasing, never reused by MaxIoUTracker). + * Caches a full Track snapshot so downstream sinks can access bbox/semantics/confidence + * even after the track has left the active window. + */ + struct AddedObjectState { + //! EMA-filtered probability the track is a newly-added object. + double added_probability = 0.0; + //! Number of frames for which a valid containment measurement was available. + int num_frames_observed = 0; + //! Raw containment ratio from the most recent valid measurement (for logging/debugging). + float last_containment = 0.0f; + //! Latched true once the track passes the added gate; prevents pruning of confirmed objects. + bool ever_added = false; + //! frame_index_ value at the last frame this track was measured (for staleness-based pruning). + int last_updated_frame = 0; + //! Full Track snapshot, refreshed each frame the track is in the active window. + Track last_track; + }; + + using ActiveWindowCDSink = hydra::OutputSink&, + const std::vector&, + const Eigen::Isometry3d&>; + + // Config. + struct Config : hydra::VerbosityConfig { + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + + //! Path to prior map to use for change detection. + std::filesystem::path prior_map_path; + + //! Ratio of free prior map points of an object's vertices in a single frame. + //! Used as raw input to the EMA filter; not the final removal decision threshold. + float removal_vertex_free_ratio_threshold = 0.8f; + + //! EMA smoothing factor for the per-object free_probability filter (0 < alpha <= 1). + //! Lower values give more smoothing (slower to react); higher values track raw ratios closely. + //! Cumulative running average (alpha=1/n) is an easy drop-in if EMA proves too noisy. + float removal_ema_alpha = 0.5f; + + //! Smoothed free_probability threshold above which an object is considered removed. + float removal_probability_threshold = 0.5f; + + //! Minimum number of frames with valid measurements before an object can be declared removed. + //! Guards against single-frame spikes on newly-entered objects. + int removal_min_frames_observed = 3; + + //! Raw per-frame containment ratio fed into the EMA filter for newly-added-object detection. + //! Not the final decision threshold; that role is played by added_probability_threshold. + float added_object_containment_threshold = 0.5f; + + //! EMA smoothing factor for the per-track added_probability filter (0 < alpha <= 1). + //! Lower = smoother / slower to react; higher = tracks raw ratios closely. + float added_ema_alpha = 0.5f; + + //! Smoothed added_probability threshold above which a track is considered a newly-added object. + float added_probability_threshold = 0.5f; + + //! Minimum valid measurements before a track can be declared newly-added. + int added_min_frames_observed = 3; + + //! Frames of no measurement after which a non-added track's state record is pruned. + //! Tracks that ever crossed the threshold are never pruned. + int added_prune_after_frames = 50; + + //! Sinks for the change detector output. + std::vector awcd_sinks; + + //! Plugin that provides the odom_T_prior (current_T_prior) transform each frame. + config::VirtualConfig transformation_getter{ + IdentityTransformationGetter::Config{}}; + + //! Enable ICP refinement on the transform returned by transformation_getter. + bool enable_icp_refinement = false; + + //! If true, invert the roman LC transform before using as ICP initial guess. + //! Verify at runtime: if ICP delta is > ~2m, flip this flag. + bool invert_roman_lc_transform = false; + + //! Crop radius around robot (m) for mesh point selection. + float icp_crop_radius = 10.0f; + + //! small_gicp threads. + size_t icp_num_threads = 2; + + //! Voxel downsampling resolution (m). + float icp_downsampling_resolution = 0.2f; + + //! Max ICP correspondence distance (m). + float icp_max_correspondence_distance = 1.0f; + + //! Min inliers to accept refined transform. + size_t icp_min_inliers = 50; + + } const config; + + // Construction. + explicit ActiveWindowChangeDetector(const Config& config); + virtual ~ActiveWindowChangeDetector() = default; + + // Module setup. + /** + * @brief Add a sink to the active window. The sink will be called whenever the active window + * finishes processing a frame. + * @param sink The sink to add. + */ + void addKhronosSink(const ActiveWindowCDSink::Ptr& sink); + + /** + * @brief TODO(multy): documentation + * @param map The current volumetric map that the active window is building. + * @param data The current data after processing. + * @param tracks The current tracks in the active window. + */ + void call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const override; + + + + void loadPriorMap(); + + bool isPriorPointFree(const Point& point_in_map, const VolumetricMap& map) const; + + bool isPointKnown(const Point& point_in_map, const VolumetricMap& map) const; + + /** + * @brief Check if a point is within allocated map bounds (has an allocated block). + * @param point The point to check in world frame. + * @param map The volumetric map to check against. + * @return True if the point is within an allocated block of the map. + */ + bool isPointInMapBounds(const Point& point, const VolumetricMap& map) const; + + /** + * @brief Find all prior object nodes whose centroid is within the current volumetric map bounds. + * @param map The current volumetric map. + * @return Vector of node IDs for objects within map bounds. + */ + std::vector findPriorObjectsInMapBounds(const VolumetricMap& map) const; + + /** + * @brief Set the transform from prior map frame to current map frame. + * @param current_T_prior Transform that converts points from prior map frame to current frame. + */ + void setCurrentToPriorTransform(const Eigen::Isometry3d& current_T_prior) const; + + /** + * @brief Transform a point from prior map frame to current map frame. + * @param point_in_prior Point in the prior map's world frame. + * @return Point transformed to the current map's world frame. + */ + Point transformPriorToCurrentFrame(const Eigen::Vector3d& point_in_prior) const; + + protected: + /// Runs small_gicp ICP using current map mesh vs prior DSG background mesh. + /// Updates current_T_prior_ if ICP converges with sufficient inliers. + void runIcpRefinement(const FrameData& data, + const VolumetricMap& map, + const Eigen::Isometry3d& initial) const; + + /** + * @brief Compute the per-frame free ratio for each candidate object with a valid measurement + * (num_known_vertices > 0). Objects with an empty mesh or no KhronosObjectAttributes are skipped. + * @return Map from NodeId to raw free ratio [0, 1] for objects that had a valid measurement. + */ + std::unordered_map computeFreeRatios( + const std::vector& objects_id_in_bounds, + const VolumetricMap& map) const; + + /** + * @brief Update the EMA filter state for each object that has a measurement this frame, + * then return the set of objects whose smoothed free_probability passes the removal gate + * (probability >= removal_probability_threshold && frames >= removal_min_frames_observed). + * Objects not in measurements are left untouched (freeze-last policy). The first time an + * object passes the gate, its RemovedObjectState::first_removed_ns is latched to `stamp` + * (the current frame's sensor time) and never updated again. + * @param measurements Per-object raw free ratio measurements for this frame. + * @param stamp Sensor frame time (ns) of this call, used to latch first_removed_ns. + */ + std::vector updateRemovedFilter( + const std::unordered_map& measurements, TimeStamp stamp) const; + + /** + * @brief Compute the per-frame containment ratio for each eligible track (non-dynamic, + * non-empty footprint). Returns a map from track ID to raw containment ratio [0, 1]. + * Reuses getPriorFreeFootprint2D and getTrackFootprint2D. + */ + std::unordered_map computeContainmentRatios(const Tracks& tracks, + const VolumetricMap& map) const; + + /** + * @brief Update the EMA filter state for each measured track, refresh cached Track snapshots, + * prune stale non-added records, then return the filtered set of Track snapshots for tracks + * that pass the added gate (added_probability >= threshold && frames >= min_frames_observed). + * Tracks absent from measurements are frozen (cached state and snapshot kept as-is). + */ + std::vector updateAddedFilter(const std::unordered_map& measurements, + const Tracks& tracks) const; + + // Returns 2D voxel indices (z forced to 0, in current frame) covering the prior + // traversable (MESH_PLACES / TravNodeAttributes) footprint within map bounds. + GlobalIndexSet getPriorFreeFootprint2D(const VolumetricMap& map) const; + + // Returns 2D voxel indices (z forced to 0) of a track's last_points footprint. + GlobalIndexSet getTrackFootprint2D(const Track& track, float voxel_size) const; + + // Convert a 3-D point to a 2D voxel GlobalIndex (z component set to 0). + static GlobalIndex to2DIndex(const Point& point, float voxel_size_inv); + + private: + //! Prior map as a 3D scene graph. + DynamicSceneGraph::Ptr prior_graph_; + + //! Transform from prior map frame to current map frame. + //! current_point = current_T_prior_ * prior_point + mutable Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); + + //! Per-object EMA filter state for removed-object detection. Keyed by prior DSG NodeId. + //! Populated lazily on first observation; never erased (freeze-last for unobserved objects). + mutable std::unordered_map removed_object_states_; + + //! Per-track EMA filter state for newly-added-object detection. Keyed by Track::id (monotonic). + //! Pruned for non-added tracks that go stale; confirmed-added records are kept forever. + mutable std::unordered_map added_object_states_; + + //! Frame counter incremented once per call(), used for staleness-based pruning of added records. + mutable int frame_index_ = 0; + + //! Plugin that provides the odom_T_prior transform. + TransformationGetter::Ptr transformation_getter_; + + //! Sinks for the change detector output. + ActiveWindowCDSink::List sinks_; +}; + +void declare_config(ActiveWindowChangeDetector::Config& config); + +} // namespace khronos diff --git a/khronos/include/khronos/active_window/change_detection/transformation_getter.h b/khronos/include/khronos/active_window/change_detection/transformation_getter.h new file mode 100644 index 0000000..1b45ed1 --- /dev/null +++ b/khronos/include/khronos/active_window/change_detection/transformation_getter.h @@ -0,0 +1,92 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +namespace khronos { + +/** + * @brief Abstract interface for obtaining the current_T_prior transform. + * + * Implementations may query TF, return identity, or use any other source. + * Returns std::nullopt when no valid transform is available (e.g. TF lookup + * failure, or change below threshold); the caller skips the update in that case. + */ +class TransformationGetter { + public: + using Ptr = std::unique_ptr; + virtual ~TransformationGetter() = default; + + /** + * @brief Get the latest odom_T_prior (current_T_prior) transform. + * @return The transform, or nullopt if unavailable / no significant change. + */ + virtual std::optional getTransformation() const = 0; +}; + +/** + * @brief Always returns Eigen::Isometry3d::Identity() — useful when no prior + * map relocalization is needed (prior and current frames are the same). + */ +class IdentityTransformationGetter : public TransformationGetter { + public: + struct Config : hydra::VerbosityConfig { + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + } const config; + + explicit IdentityTransformationGetter(const Config& config); + + /** + * @brief Returns nullopt — the transform remains at its default (Identity). + * Use this getter when no relocalization is needed (prior and current frames + * are the same). + */ + std::optional getTransformation() const override; +}; + +void declare_config(IdentityTransformationGetter::Config& config); + +} // namespace khronos diff --git a/khronos/include/khronos/active_window/data/frame_data.h b/khronos/include/khronos/active_window/data/frame_data.h index e051806..3c2414c 100644 --- a/khronos/include/khronos/active_window/data/frame_data.h +++ b/khronos/include/khronos/active_window/data/frame_data.h @@ -48,6 +48,10 @@ #include "khronos/active_window/data/measurement_clusters.h" #include "khronos/common/common_types.h" +namespace hydra { +class Camera; +} // namespace hydra + namespace khronos { using hydra::InputData; @@ -80,6 +84,57 @@ struct FrameData { using ObjectImageType = int; explicit FrameData(const hydra::InputData& input) : input(input) {} + + /** + * @brief Serialize this frame's essential-for-tracking fields to `observation_dir` (created if + * needed): color/depth images, pose, and the full per-frame segmentation (object_image/ + * dynamic_image + clusters.json with each cluster's real id/semantics/bounding_box). Does not + * save camera intrinsics -- see saveCameraIntrinsics/loadCameraIntrinsics below; the sensor is a + * run-level constant, not part of any single frame. + * @param observation_dir Destination directory, e.g. ".../observations/". + */ + void save(const std::string& observation_dir) const; + + /** + * @brief Reconstruct a FrameData from a directory written by save(), given the sensor to + * associate it with (see loadCameraIntrinsics) and its timestamp. + * @param observation_dir Directory written by save(), e.g. ".../observations/". + * @param camera Sensor to associate with the reconstructed input (intrinsics + identity + * extrinsics; see loadCameraIntrinsics). + * @param stamp Timestamp (ns) to assign to the reconstructed frame. + * @param min_range Minimum depth [m] for a pixel to be included in a cluster's pixels; mirrors + * instance_forwarding.cpp's own pixel-validity gate (`min_range: 0.05` in the deployed config). + * 0 (default) disables this check. Non-finite depth is always excluded regardless of this value + * (no such guard exists in the production pipeline that builds cluster.pixels, so this + * reconstruction filters explicitly to keep bbox/IoU computations well-defined). + * @param max_range Maximum depth [m] for a pixel to be included; mirrors instance_forwarding.cpp + * (`max_range: 10.0` in the deployed config). 0 (default) disables this check. + * @return The reconstructed FrameData, or nullptr if any required file is missing/malformed. + */ + static FrameData::Ptr load(const std::string& observation_dir, + const std::shared_ptr& camera, + TimeStamp stamp, + float min_range = 0.f, + float max_range = 0.f); }; +/** + * @brief Save the run-level camera intrinsics once (not part of any single FrameData::save() + * call -- the sensor is constant across all frames from the same run). Free functions (not + * members of FrameData or Camera) since hydra::Camera is not a khronos-owned type. + * @param camera Sensor to persist. + * @param run_dir Run root directory, e.g. ".../khronos_tracks_". + */ +void saveCameraIntrinsics(const hydra::Camera& camera, const std::string& run_dir); + +/** + * @brief Load the run-level camera intrinsics saved by saveCameraIntrinsics(), constructing a + * Camera with 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). + * @param run_dir Run root directory, e.g. ".../khronos_tracks_". + * @return The reconstructed Camera, or nullptr if camera_intrinsics.json is missing/malformed. + */ +std::shared_ptr loadCameraIntrinsics(const std::string& run_dir); + } // namespace khronos diff --git a/khronos/include/khronos/active_window/data/track.h b/khronos/include/khronos/active_window/data/track.h index e56e8b3..c4ee555 100644 --- a/khronos/include/khronos/active_window/data/track.h +++ b/khronos/include/khronos/active_window/data/track.h @@ -38,6 +38,7 @@ #pragma once #include +#include #include #include "khronos/active_window/data/measurement_clusters.h" @@ -108,6 +109,21 @@ struct Track { bool is_active = true; void updateSemantics(const std::optional& other_semantics); + + /** + * @brief Serialize this track (all fields consumed by trackers, e.g. MaxIoUTracker) to a JSON + * file at the given path. `is_active` is intentionally not persisted: reconstructed tracks are + * meant to be fed into a tracker for offline replay, where activity is re-derived, not restored. + * @param filepath Destination path, e.g. ".../track_/track.json". + */ + void save(const std::string& filepath) const; + + /** + * @brief Reconstruct a Track from a JSON file written by save(). + * @param filepath Path to a track.json file. + * @return The reconstructed Track. + */ + static Track load(const std::string& filepath); }; using Tracks = std::vector; diff --git a/khronos/include/khronos/active_window/object_extraction/mesh_object_extractor.h b/khronos/include/khronos/active_window/object_extraction/mesh_object_extractor.h index 6483762..f735052 100644 --- a/khronos/include/khronos/active_window/object_extraction/mesh_object_extractor.h +++ b/khronos/include/khronos/active_window/object_extraction/mesh_object_extractor.h @@ -96,6 +96,9 @@ class MeshObjectExtractor : public ObjectExtractor { // for debugging. bool visualize_classification = false; + // Bounding box type used for extracted (persistent) static objects. + enum class BBoxType { kAABB, kRAABB } bbox_type = BBoxType::kAABB; + hydra::SensorMap::Config projective_integrator; hydra::MeshIntegratorConfig mesh_integrator; } const config; diff --git a/khronos/include/khronos/active_window/track_saver_sink.h b/khronos/include/khronos/active_window/track_saver_sink.h new file mode 100644 index 0000000..30ad661 --- /dev/null +++ b/khronos/include/khronos/active_window/track_saver_sink.h @@ -0,0 +1,143 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include +#include + +#include "khronos/active_window/active_window.h" + +namespace khronos { + +/** + * @brief Active-window KhronosSink that dumps raw per-frame observation data (color/depth/pose + + * full per-frame segmentation, via FrameData::save) once per real frame into a shared + * `observations//` folder, and per-track state (track.json, mask/overlay crops, point + * clouds) into `tracks/track_/` folders that symlink back to the shared observation data + * instead of duplicating it. Used for offline debugging of tracker fragmentation (see + * tracker_debug.md): the observations/ folder is a complete, algorithm-agnostic record of every + * frame (saved unconditionally, regardless of whether any track claims it), so alternate + * detector/tracker configs can be replayed against it later. + */ +class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { + public: + // Config. + struct Config : hydra::VerbosityConfig { + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + + //! Base output directory. A timestamped subdirectory (khronos_tracks_) is + //! created underneath it on first use to avoid collisions between runs. + std::string output_directory; + + //! Save observations// (color/depth/pose + full per-frame segmentation) for every + //! frame, via FrameData::save. Unconditional per frame (not gated on tracker output) so the + //! saved data can be replayed against a different detector/tracker config later. + bool save_observations = true; + + //! Save the binary instance mask crop for the track's observation this frame. + bool save_masks = true; + + //! Save the camera intrinsics once per run (run_dir/camera_intrinsics.json). + bool save_camera_intrinsics = true; + + //! Save a masked, colored 3D point cloud (.ply) reconstructed from the vertex map this frame. + bool save_pointcloud = false; + + //! Reference frame the saved point cloud is expressed in. One of "world" (absolute map frame, + //! matches the raw vertex_map), "robot"/"body" (relative to the current world_T_body pose, so + //! coincident tracks show up near the origin), or "sensor" (relative to the sensor optical pose). + std::string pointcloud_frame = "world"; + + //! Save/overwrite a track.json with the full serialized Track (see Track::save), enough to + //! reconstruct the Track object for offline tracker replay. + bool save_track_json = true; + + //! Create symlinks in each track_/ folder pointing back to the shared observations// + //! color/depth/pose files, for convenient single-folder browsing without duplicating the data. + bool save_track_symlinks = true; + } const config; + + // Construction. + explicit ActiveWindowTrackSaver(const Config& config); + virtual ~ActiveWindowTrackSaver() = default; + + /** + * @brief Saves this frame's observation data (once, unconditionally) and, for every track + * observed this frame, its track-specific state. + * @param data The current frame data (only the current frame is available; no history buffer). + * @param map The current volumetric map (unused; present for KhronosSink interface). + * @param tracks The current tracks in the active window. + */ + void call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const override; + + private: + //! Lazily create (once) and return the timestamped base output directory for this run. + std::string getBaseDir() const; + + //! Directory for a specific track, creating it if needed. + std::string getTrackDir(int track_id) const; + + void saveMask(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + int semantic_cluster_id, + cv::Mat* binary_mask_out) const; + void saveOverlay(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + const cv::Mat* binary_mask) const; + void savePointcloud(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + const cv::Mat* binary_mask) const; + void saveTrackJson(const std::string& track_dir, const Track& track) const; + void createTrackSymlinks(const std::string& track_dir, const std::string& ts_str) const; + + //! Cached base output directory for this run, computed lazily on first call(). + mutable std::string base_dir_; + + //! Whether camera_intrinsics.json has already been written for this run. + mutable bool intrinsics_saved_ = false; +}; + +void declare_config(ActiveWindowTrackSaver::Config& config); + +} // namespace khronos diff --git a/khronos/include/khronos/active_window/tracking/max_iou_tracker.h b/khronos/include/khronos/active_window/tracking/max_iou_tracker.h index 2608531..f81e2b3 100644 --- a/khronos/include/khronos/active_window/tracking/max_iou_tracker.h +++ b/khronos/include/khronos/active_window/tracking/max_iou_tracker.h @@ -38,6 +38,7 @@ #pragma once #include +#include #include #include #include @@ -90,6 +91,8 @@ class MaxIoUTracker : public Tracker { // Voxel size in meters used for tracking. Only used if 'track_by' is 'voxels'. float voxel_size = 0.1f; + + enum class BBoxType { kAABB, kRAABB } bbox_type = BBoxType::kAABB; } const config; // Construction. @@ -131,6 +134,17 @@ class MaxIoUTracker : public Tracker { float computeIoUPixels(const FrameData& data, const MeasurementCluster& cluster, const Track& track) const; + /** + * @brief Reproject a set of 3D world points into the given frame's sensor image plane. + * 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. + * @param data The frame to reproject into (uses its sensor pose and sensor model). + * @param points 3D world points to reproject (e.g. a track's last_points). + * @return The set of image pixels the points project onto (points that fail to project, e.g. + * behind the camera, are omitted). + */ + std::set reprojectPoints(const FrameData& data, const Points& points) const; float computeIoUBoundingBox(const FrameData& data, const MeasurementCluster& cluster, const Track& track) const; diff --git a/khronos/include/khronos/utils/icp_registration_utils.h b/khronos/include/khronos/utils/icp_registration_utils.h new file mode 100644 index 0000000..b186c03 --- /dev/null +++ b/khronos/include/khronos/utils/icp_registration_utils.h @@ -0,0 +1,79 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include + +namespace khronos { + +/** + * @brief Utilities for ICP registration of point clouds + */ +class ICPRegistrationUtils { + public: + //! @brief Clone of small_gicp result structure to avoid public include + struct Result { + Eigen::Isometry3d T_target_source = Eigen::Isometry3d::Identity(); + bool converged = false; + size_t iterations = 0; + size_t num_inliers = 0; + Eigen::Matrix H; + Eigen::Matrix b; + double error = 0.0; + }; + + /** + * @brief Perform ICP registration between two point clouds + * + * @param source_points Source point cloud + * @param target_points Target point cloud + * @param num_threads Number of threads to use for registration (default: 1) + * @param downsampling_resolution Voxel size for downsampling (meters) + * @param max_correspondence_distance Maximum distance for point correspondences (meters) + * @return Registration result containing transformation and convergence info + */ + static Result registerPointClouds(const std::vector& source_points, + const std::vector& target_points, + size_t num_threads = 1, + float downsampling_resolution = 0.1f, + float max_correspondence_distance = 0.5f); +}; + +} // namespace khronos diff --git a/khronos/include/khronos/utils/json_utils.h b/khronos/include/khronos/utils/json_utils.h new file mode 100644 index 0000000..84f513a --- /dev/null +++ b/khronos/include/khronos/utils/json_utils.h @@ -0,0 +1,62 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include + +#include "khronos/active_window/data/measurement_clusters.h" +#include "khronos/common/common_types.h" + +namespace khronos { + +/** + * @brief Serialize a BoundingBox to JSON manually (not via spark_dsg's to_json/from_json, since + * the latter's from_json depends on a loaded io::GlobalInfo file header, which is not available + * in this offline-debugging-tool context). + */ +nlohmann::json toJson(const BoundingBox& bbox); +BoundingBox boundingBoxFromJson(const nlohmann::json& j); + +/** + * @brief Serialize SemanticClusterInfo. The feature vector is only persisted when it actually + * carries openset info (size > 1); a size-1 feature is the "no features" placeholder. + */ +nlohmann::json toJson(const SemanticClusterInfo& semantics); +SemanticClusterInfo semanticsFromJson(const nlohmann::json& j); + +} // namespace khronos diff --git a/khronos/package.xml b/khronos/package.xml index 3fcbde3..4c2d676 100644 --- a/khronos/package.xml +++ b/khronos/package.xml @@ -22,6 +22,7 @@ spark_dsg kimera_pgmo hydra + small_gicp cmake diff --git a/khronos/src/active_window/active_window.cpp b/khronos/src/active_window/active_window.cpp index 4532b31..584beea 100644 --- a/khronos/src/active_window/active_window.cpp +++ b/khronos/src/active_window/active_window.cpp @@ -195,6 +195,18 @@ hydra::ActiveWindowOutput::Ptr ActiveWindow::spinOnce(const hydra::InputPacket& ++num_frames_processed_; Timer sink_timer("active_window/sinks", latest_stamp_); + // local change detection using sinks/module + /* + Input: + 1. vocumetric map + 2. tracks (optional) + Init: + 1. prior maps + Output: + 1. changed (add/remove) object nodes + new objects + + */ KhronosSink::callAll(sinks_, *data, map_, tracks_); sink_timer.stop(); diff --git a/khronos/src/active_window/change_detection/active_window_change_detector.cpp b/khronos/src/active_window/change_detection/active_window_change_detector.cpp new file mode 100644 index 0000000..88acea7 --- /dev/null +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -0,0 +1,597 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos/active_window/change_detection/active_window_change_detector.h" + +#include +#include +#include +#include + +#include "khronos/utils/icp_registration_utils.h" + +namespace khronos { +namespace { + +static const auto registration = config::RegistrationWithConfig( + "ActiveWindowChangeDetector"); +} + +void declare_config(ActiveWindowChangeDetector::Config& config) { + using namespace config; + name("ActiveWindowChangeDetector"); + field(config.verbosity, "verbosity"); + field(config.removal_vertex_free_ratio_threshold, "removal_vertex_free_ratio_threshold"); + field(config.removal_ema_alpha, "removal_ema_alpha"); + field(config.removal_probability_threshold, "removal_probability_threshold"); + field(config.removal_min_frames_observed, "removal_min_frames_observed"); + field(config.added_object_containment_threshold, "added_object_containment_threshold"); + field(config.added_ema_alpha, "added_ema_alpha"); + field(config.added_probability_threshold, "added_probability_threshold"); + field(config.added_min_frames_observed, "added_min_frames_observed"); + field(config.added_prune_after_frames, "added_prune_after_frames"); + field(config.prior_map_path, "prior_map_path"); + field(config.awcd_sinks, "awcd_sinks"); + field(config.transformation_getter, "transformation_getter"); + field(config.enable_icp_refinement, "enable_icp_refinement"); + field(config.invert_roman_lc_transform, "invert_roman_lc_transform"); + field(config.icp_crop_radius, "icp_crop_radius"); + field(config.icp_num_threads, "icp_num_threads"); + field(config.icp_downsampling_resolution, "icp_downsampling_resolution"); + field(config.icp_max_correspondence_distance, "icp_max_correspondence_distance"); + field(config.icp_min_inliers, "icp_min_inliers"); + check(config.prior_map_path, "prior_map_path"); + // check(config.prior_map_path, "prior_map_path", ".spark_dsg"); // Add the + // check back sometimes. +} + +ActiveWindowChangeDetector::ActiveWindowChangeDetector(const Config& config) + : config(config::checkValid(config)), + transformation_getter_(config.transformation_getter.create()), + sinks_(ActiveWindowCDSink::instantiate(config.awcd_sinks)) { + MLOG(1) << "[Khronos Active Window Change Detector] Initialized with prior map path: " + << config.prior_map_path; + loadPriorMap(); + MLOG(1) << "[Khronos Active Window Change Detector] Loaded prior scene graph with " + << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); + // print out the config + MLOG(1) << "[Khronos Active Window Change Detector] Config: " << config; +} + +void ActiveWindowChangeDetector::addKhronosSink(const ActiveWindowCDSink::Ptr& sink) { + if (sink) { + sinks_.push_back(sink); + } +} + +std::unordered_map ActiveWindowChangeDetector::computeFreeRatios( + const std::vector& objects_id_in_bounds, + const VolumetricMap& map) const { + std::unordered_map measurements; + + for (const auto object_id : objects_id_in_bounds) { + const auto& object_node = prior_graph_->getNode(object_id); + + const auto* khronos_attrs = object_node.tryAttributes(); + if (!khronos_attrs) { + MLOG(5) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " does not have KhronosObjectAttributes, skipping"; + continue; + } + + const auto& mesh = khronos_attrs->mesh; + const auto& bbox = khronos_attrs->bounding_box; + + if (mesh.numVertices() == 0) { + MLOG(5) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " has empty mesh, skipping"; + continue; + } + + int num_vertices_in_free_space = 0; + int num_vertices_in_bound_and_known = 0; + + for (size_t i = 0; i < mesh.numVertices(); ++i) { + // Transform: local → prior_world → current_world + const Eigen::Vector3f vertex_local = mesh.pos(i); + const Eigen::Vector3f vertex_prior_world = bbox.pointToWorldFrame(vertex_local); + const Point vertex_current = transformPriorToCurrentFrame(vertex_prior_world.cast()); + + if (!isPointKnown(vertex_current, map)) { + continue; // Skip unknown points — no valid measurement at this vertex. + } + + ++num_vertices_in_bound_and_known; + + if (isPriorPointFree(vertex_current, map)) { + ++num_vertices_in_free_space; + } + } + + // Guard division: if no known vertices observed this frame, there is no valid measurement. + // Skip to avoid feeding NaN/garbage into the EMA filter. + if (num_vertices_in_bound_and_known == 0) { + MLOG(5) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " has no known vertices this frame, skipping measurement"; + continue; + } + + const float free_ratio = static_cast(num_vertices_in_free_space) / + static_cast(num_vertices_in_bound_and_known); + + measurements[object_id] = free_ratio; + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " raw free ratio: " << free_ratio; + } + + return measurements; +} + +std::vector ActiveWindowChangeDetector::updateRemovedFilter( + const std::unordered_map& measurements, TimeStamp stamp) const { + // Update EMA state for each object that has a measurement this frame. + // Objects absent from measurements are left untouched (freeze-last policy). + for (const auto& [object_id, free_ratio] : measurements) { + auto& state = removed_object_states_[object_id]; + + if (state.num_frames_observed == 0) { + // First observation: initialize the filter to the raw measurement. + state.free_probability = free_ratio; + } else { + // EMA update: p = alpha * free_ratio + (1 - alpha) * p + state.free_probability = + config.removal_ema_alpha * free_ratio + (1.0 - config.removal_ema_alpha) * state.free_probability; + } + ++state.num_frames_observed; + state.last_free_ratio = free_ratio; + + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " EMA update: raw=" << free_ratio + << " smoothed=" << state.free_probability + << " frames=" << state.num_frames_observed; + } + + // Threshold the full state map (not just this frame's measurements) to build the removed set. + // This means objects that leave the map temporarily retain their last smoothed probability. + std::vector removed_objects; + for (auto& [object_id, state] : removed_object_states_) { + const bool above_prob = state.free_probability >= config.removal_probability_threshold; + const bool enough_frames = state.num_frames_observed >= config.removal_min_frames_observed; + if (above_prob && enough_frames) { + // Latch the first sensor time this object was declared removed. Never reset, even if + // the object later drops out of the removed set and returns. + if (state.first_removed_ns == 0) { + state.first_removed_ns = stamp; + } + removed_objects.push_back(RemovedObject{object_id, state.first_removed_ns}); + MLOG(3) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " REMOVED (p=" << state.free_probability + << ", frames=" << state.num_frames_observed << ")"; + } else { + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " not removed (p=" << state.free_probability + << ", frames=" << state.num_frames_observed << ")"; + } + } + + MLOG(2) << "[ActiveWindowChangeDetector] Removed-object filter: " + << removed_objects.size() << " removed out of " + << removed_object_states_.size() << " tracked candidates"; + + return removed_objects; +} + +GlobalIndex ActiveWindowChangeDetector::to2DIndex(const Point& point, float voxel_size_inv) { + GlobalIndex idx = spatial_hash::indexFromPoint(point, voxel_size_inv); + idx.z() = 0; + return idx; +} + +GlobalIndexSet ActiveWindowChangeDetector::getPriorFreeFootprint2D( + const VolumetricMap& map) const { + GlobalIndexSet free_footprint; + + if (!prior_graph_ || !prior_graph_->hasLayer(DsgLayers::MESH_PLACES)) { + LOG(WARNING) << "[ActiveWindowChangeDetector] Prior graph has no MESH_PLACES layer; " + "newly-added object detection requires traversability places."; + return free_footprint; + } + + const auto& places_layer = prior_graph_->getLayer(DsgLayers::MESH_PLACES); + const float voxel_size = map.config.voxel_size; + const float voxel_size_inv = 1.0f / voxel_size; + // Step at half voxel size in prior frame to avoid coverage gaps after transform. + const float step = voxel_size * 0.5f; + + int num_trav_nodes = 0; + for (const auto& [node_id, node] : places_layer.nodes()) { + const auto* attrs = node->tryAttributes(); + if (!attrs) { + continue; + } + ++num_trav_nodes; + + // Filter to places whose center falls inside the current active-window map. + const Point center_current = + transformPriorToCurrentFrame(attrs->position); + if (!isPointInMapBounds(center_current, map)) { + continue; + } + + // Rasterize the place's footprint in prior-map frame, then index in current frame. + const double max_r = attrs->max_radius; + const double cx = attrs->position.x(); + const double cy = attrs->position.y(); + const double cz = attrs->position.z(); + + for (double x = cx - max_r; x <= cx + max_r; x += step) { + for (double y = cy - max_r; y <= cy + max_r; y += step) { + const Eigen::Vector3d candidate_prior(x, y, cz); + if (!attrs->contains(candidate_prior)) { + continue; + } + const Point pt_current = transformPriorToCurrentFrame(candidate_prior); + free_footprint.insert(to2DIndex(pt_current, voxel_size_inv)); + } + } + } + + if (num_trav_nodes == 0) { + LOG(WARNING) << "[ActiveWindowChangeDetector] MESH_PLACES layer exists but contains no " + "TravNodeAttributes nodes. Prior map may have been built without traversability " + "places; newly-added object detection will be a no-op."; + } + + MLOG(3) << "[ActiveWindowChangeDetector] Prior free 2D footprint: " << free_footprint.size() + << " voxels from " << num_trav_nodes << " traversability place nodes."; + return free_footprint; +} + +GlobalIndexSet ActiveWindowChangeDetector::getTrackFootprint2D(const Track& track, + float voxel_size) const { + GlobalIndexSet footprint; + const float voxel_size_inv = 1.0f / voxel_size; + for (const Point& pt : track.last_points) { + footprint.insert(to2DIndex(pt, voxel_size_inv)); + } + return footprint; +} + +std::unordered_map ActiveWindowChangeDetector::computeContainmentRatios( + const Tracks& tracks, + const VolumetricMap& map) const { + std::unordered_map measurements; + + // Build the 2D free-space footprint from prior traversability places. + const GlobalIndexSet prior_free = getPriorFreeFootprint2D(map); + if (prior_free.empty()) { + return measurements; + } + + const float voxel_size = map.config.voxel_size; + + for (const Track& track : tracks) { + // Skip dynamic objects and tracks with no point observations. + if (track.is_dynamic || track.last_points.empty()) { + continue; + } + + const GlobalIndexSet track2D = getTrackFootprint2D(track, voxel_size); + if (track2D.empty()) { + continue; + } + + // Containment ratio: fraction of the track's 2D footprint that falls in prior free space. + int intersection = 0; + for (const GlobalIndex& idx : track2D) { + if (prior_free.count(idx)) { + ++intersection; + } + } + const float containment = static_cast(intersection) / static_cast(track2D.size()); + + measurements[track.id] = containment; + MLOG(4) << "[ActiveWindowChangeDetector] Track " << track.id + << " raw containment: " << containment; + } + + return measurements; +} + +std::vector ActiveWindowChangeDetector::updateAddedFilter( + const std::unordered_map& measurements, + const Tracks& tracks) const { + ++frame_index_; + + // Build a quick lookup from track ID to Track pointer for snapshot refresh. + std::unordered_map track_lookup; + track_lookup.reserve(tracks.size()); + for (const Track& t : tracks) { + track_lookup[t.id] = &t; + } + + // Update EMA state for each track that has a measurement this frame. + // Tracks absent from measurements are left untouched (freeze-last policy). + for (const auto& [track_id, containment] : measurements) { + auto& state = added_object_states_[track_id]; + + if (state.num_frames_observed == 0) { + // First observation: initialize the filter to the raw measurement. + state.added_probability = containment; + } else { + // EMA update: p = alpha * containment + (1 - alpha) * p + state.added_probability = + config.added_ema_alpha * containment + + (1.0 - config.added_ema_alpha) * state.added_probability; + } + ++state.num_frames_observed; + state.last_containment = containment; + state.last_updated_frame = frame_index_; + + // Refresh the cached Track snapshot. + auto it = track_lookup.find(track_id); + if (it != track_lookup.end()) { + state.last_track = *(it->second); + } + + MLOG(4) << "[ActiveWindowChangeDetector] Track " << track_id + << " EMA update: raw=" << containment + << " smoothed=" << state.added_probability + << " frames=" << state.num_frames_observed; + } + + // Threshold the full state map and collect confirmed-added Track snapshots. + // Prune stale non-added records along the way. + std::vector newly_added_tracks; + for (auto it = added_object_states_.begin(); it != added_object_states_.end();) { + auto& [track_id, state] = *it; + + const bool above_prob = state.added_probability >= config.added_probability_threshold; + const bool enough_frames = state.num_frames_observed >= config.added_min_frames_observed; + + if (above_prob && enough_frames) { + state.ever_added = true; + newly_added_tracks.push_back(state.last_track); + MLOG(3) << "[ActiveWindowChangeDetector] Track " << track_id + << " ADDED (p=" << state.added_probability + << ", frames=" << state.num_frames_observed << ")"; + ++it; + } else if (!state.ever_added && + (frame_index_ - state.last_updated_frame) > config.added_prune_after_frames) { + // Stale non-added record — prune it. + MLOG(4) << "[ActiveWindowChangeDetector] Pruning stale non-added track " << track_id; + it = added_object_states_.erase(it); + } else { + MLOG(4) << "[ActiveWindowChangeDetector] Track " << track_id + << " not added (p=" << state.added_probability + << ", frames=" << state.num_frames_observed << ")"; + ++it; + } + } + + MLOG(2) << "[ActiveWindowChangeDetector] Added-object filter: " + << newly_added_tracks.size() << " added out of " + << added_object_states_.size() << " tracked candidates"; + + return newly_added_tracks; +} + +void ActiveWindowChangeDetector::call(const FrameData& data, + const VolumetricMap& map, + const Tracks& tracks) const { + // Poll the transformation getter and update current_T_prior_ if a new transform is available. + const auto tf = transformation_getter_->getTransformation(); + if (tf.has_value()) { + if (config.enable_icp_refinement) { + runIcpRefinement(data, map, tf.value()); + } else { + setCurrentToPriorTransform(tf.value()); + } + } + + // 1. Find all object nodes in the prior graph within current volumetric map bounds. + const auto objects_id_in_bounds = findPriorObjectsInMapBounds(map); + + // 2. Compute per-frame free ratios for objects with valid measurements. + const auto free_ratio_measurements = computeFreeRatios(objects_id_in_bounds, map); + + // 3. Update EMA filter and threshold to obtain the temporally-filtered removed set. + const auto removed_objects = updateRemovedFilter(free_ratio_measurements, data.input.timestamp_ns); + + // 4. Compute per-frame containment ratios for newly-added-object detection. + // Known limitation: objects on tables cannot be detected this way (footprint-on-ground heuristic). + const auto containment_measurements = computeContainmentRatios(tracks, map); + + // 5. Update EMA filter, prune stale records, and return filtered Track snapshots. + const auto newly_added_tracks = updateAddedFilter(containment_measurements, tracks); + + MLOG(2) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() + << " removed objects and " << newly_added_tracks.size() << " newly added tracks."; + + // Call all sinks with removed objects, newly-added tracks, and the prior-to-current transform. + ActiveWindowCDSink::callAll( + sinks_, prior_graph_, removed_objects, newly_added_tracks, current_T_prior_); +} + +void ActiveWindowChangeDetector::loadPriorMap() { + // NOTE(multy): required to load the full path to the DSG file. + // TODO(multy): in documentation might require to set a prior map path that's different from the + // DCIST env variable. + prior_graph_ = DynamicSceneGraph::load(config.prior_map_path); + MLOG(1) << "[ActiveWindowChangeDetector] Loaded prior graph from " << config.prior_map_path + << " with " + << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); +} + +bool ActiveWindowChangeDetector::isPriorPointFree(const Point& point_in_map, + const VolumetricMap& map) const { + const auto* voxel = map.getTrackingLayer()->getVoxelPtr(point_in_map); + return voxel && voxel->ever_free; +} + +bool ActiveWindowChangeDetector::isPointInMapBounds(const Point& point, + const VolumetricMap& map) const { + // A point is "in bounds" if the map has an allocated block at that location + const auto& tsdf_layer = map.getTsdfLayer(); + return tsdf_layer.hasBlock(point.cast()); +} + +bool ActiveWindowChangeDetector::isPointKnown(const Point& point_in_map, + const VolumetricMap& map) const { + const auto* voxel = map.getTrackingLayer()->getVoxelPtr(point_in_map); + return voxel && voxel->last_observed != 0u; +} + +std::vector ActiveWindowChangeDetector::findPriorObjectsInMapBounds( + const VolumetricMap& map) const { + std::vector objects_in_bounds; + + if (!prior_graph_ || !prior_graph_->hasLayer(DsgLayers::OBJECTS)) { + LOG(WARNING) << "[ActiveWindowChangeDetector] Prior graph has no OBJECTS layer"; + return objects_in_bounds; + } + + const auto& objects_layer = prior_graph_->getLayer(DsgLayers::OBJECTS); + + for (const auto& [node_id, node] : objects_layer.nodes()) { + const auto& attrs = node->attributes(); + // Transform position from prior map frame to current map frame + const Point position_in_current = transformPriorToCurrentFrame(attrs.position); + + if (isPointInMapBounds(position_in_current, map)) { + objects_in_bounds.push_back(node_id); + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(node_id).str() + << " is within map bounds at position (current frame): " + << position_in_current.transpose(); + } + } + + MLOG(4) << "[ActiveWindowChangeDetector] Found " << objects_in_bounds.size() + << " prior objects within current map bounds"; + + return objects_in_bounds; +} + +void ActiveWindowChangeDetector::setCurrentToPriorTransform( + const Eigen::Isometry3d& current_T_prior) const { + current_T_prior_ = current_T_prior; + MLOG(1) << "[ActiveWindowChangeDetector] Updated current_T_prior transform:\n" + << " Translation: " << current_T_prior_.translation().transpose() << "\n" + << " Rotation (quaternion wxyz): " + << Eigen::Quaterniond(current_T_prior_.rotation()).coeffs().transpose(); +} + +Point ActiveWindowChangeDetector::transformPriorToCurrentFrame( + const Eigen::Vector3d& point_in_prior) const { + // TODO(multy): In the future, consider transforming the prior map once to the current map frame + // instead of transforming each point. + // Transform: current_point = current_T_prior * prior_point + const Eigen::Vector3d point_in_current = current_T_prior_ * point_in_prior; + return point_in_current.cast(); +} + +void ActiveWindowChangeDetector::runIcpRefinement(const FrameData& data, + const VolumetricMap& map, + const Eigen::Isometry3d& initial) const { + if (!prior_graph_ || !prior_graph_->mesh() || prior_graph_->mesh()->points.empty()) { + LOG(WARNING) << "[ActiveWindowChangeDetector] No prior background mesh for ICP."; + return; + } + + const Eigen::Vector3f robot_cur = data.input.world_T_body.translation().cast(); + const Eigen::Isometry3f prior_T_current = initial.inverse().cast(); + const Eigen::Vector3f robot_prior = prior_T_current * robot_cur; + const float r = config.icp_crop_radius; + + // Collect current mesh points and pre-transform to prior frame. + std::vector source; + for (const auto& block : map.getMeshLayer()) { + for (const auto& pt : block.points) { + if ((pt - robot_cur).norm() <= r) { + source.push_back(prior_T_current * pt); + } + } + } + + // Collect prior background mesh points near robot in prior frame. + std::vector target; + for (const auto& pt : prior_graph_->mesh()->points) { + if ((pt - robot_prior).norm() <= r) { + target.push_back(pt); + } + } + + if (source.empty() || target.empty()) { + LOG(WARNING) << "[ActiveWindowChangeDetector] Insufficient mesh points for ICP."; + return; + } + MLOG(1) << "[ActiveWindowChangeDetector] ICP: " << source.size() << " src, " << target.size() + << " tgt pts."; + + const auto res = + ICPRegistrationUtils::registerPointClouds(source, + target, + config.icp_num_threads, + config.icp_downsampling_resolution, + config.icp_max_correspondence_distance); + + if (!res.converged || res.num_inliers < config.icp_min_inliers) { + LOG(WARNING) << "[ActiveWindowChangeDetector] ICP failed: converged=" << res.converged + << " inliers=" << res.num_inliers; + return; + } + + setCurrentToPriorTransform(initial * res.T_target_source.inverse()); + MLOG(1) << "[ActiveWindowChangeDetector] ICP refined transform. Inliers: " << res.num_inliers + << ", translation delta: " + << (current_T_prior_.translation() - initial.translation()).norm() << " m."; + + // print initial guess and refined transform for debugging in x,y,z and euler angles x,y,z + const Eigen::Vector3d initial_trans = initial.translation(); + const Eigen::Vector3d initial_euler = initial.rotation().eulerAngles(0, 1, 2); + const Eigen::Vector3d refined_trans = current_T_prior_.translation(); + const Eigen::Vector3d refined_euler = current_T_prior_.rotation().eulerAngles(0, 1, 2); + + MLOG(1) << "[ActiveWindowChangeDetector] Initial guess - Translation (x,y,z): " + << initial_trans.transpose() << ", Euler angles (x,y,z): " << initial_euler.transpose(); + MLOG(1) << "[ActiveWindowChangeDetector] Refined transform - Translation (x,y,z): " + << refined_trans.transpose() << ", Euler angles (x,y,z): " << refined_euler.transpose(); +} + +} // namespace khronos diff --git a/khronos/src/active_window/change_detection/transformation_getter.cpp b/khronos/src/active_window/change_detection/transformation_getter.cpp new file mode 100644 index 0000000..da44c19 --- /dev/null +++ b/khronos/src/active_window/change_detection/transformation_getter.cpp @@ -0,0 +1,65 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos/active_window/change_detection/transformation_getter.h" + +namespace khronos { +namespace { + +static const auto registration = + config::RegistrationWithConfig( + "IdentityTransformationGetter"); + +} // namespace + +void declare_config(IdentityTransformationGetter::Config& config) { + using namespace config; + name("IdentityTransformationGetter"); + field(config.verbosity, "verbosity"); +} + +IdentityTransformationGetter::IdentityTransformationGetter(const Config& config) + : config(config::checkValid(config)) {} + +std::optional IdentityTransformationGetter::getTransformation() const { + // Returns nullopt: no relocalization needed, current_T_prior stays at Identity. + return std::nullopt; +} + +} // namespace khronos diff --git a/khronos/src/active_window/data/frame_data.cpp b/khronos/src/active_window/data/frame_data.cpp new file mode 100644 index 0000000..cec1ae4 --- /dev/null +++ b/khronos/src/active_window/data/frame_data.cpp @@ -0,0 +1,387 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos/active_window/data/frame_data.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "khronos/utils/json_utils.h" +#include "khronos/utils/output_file_utils.h" + +namespace khronos { +namespace { + +using json = nlohmann::json; + +void saveColorDepthPose(const FrameData& data, const std::string& dir) { + if (!data.input.color_image.empty()) { + cv::Mat bgr_image; + cv::cvtColor(data.input.color_image, bgr_image, cv::COLOR_RGB2BGR); + cv::imwrite(dir + "/color.png", bgr_image); + } + + if (!data.input.depth_image.empty()) { + // 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(data.input.depth_image.data), + data.input.depth_image.total() * data.input.depth_image.elemSize()); + } + } + + std::ofstream pose_stream(dir + "/pose.txt"); + if (pose_stream.is_open()) { + const Eigen::Isometry3d world_T_sensor = data.input.getSensorPose(); + pose_stream << std::fixed << std::setprecision(9); + + pose_stream << "# Sensor pose in world frame\n"; + pose_stream << "# Translation (x, y, z):\n"; + pose_stream << world_T_sensor.translation().x() << " " << world_T_sensor.translation().y() << " " + << world_T_sensor.translation().z() << "\n"; + + pose_stream << "# Rotation matrix (3x3):\n"; + const Eigen::Matrix3d rotation = world_T_sensor.rotation(); + for (int i = 0; i < 3; ++i) { + pose_stream << rotation(i, 0) << " " << rotation(i, 1) << " " << rotation(i, 2) << "\n"; + } + + pose_stream << "# Full transformation matrix (4x4):\n"; + const Eigen::Matrix4d transform = world_T_sensor.matrix(); + for (int i = 0; i < 4; ++i) { + pose_stream << transform(i, 0) << " " << transform(i, 1) << " " << transform(i, 2) << " " + << transform(i, 3) << "\n"; + } + } +} + +void saveRawInt32(const cv::Mat& image, const std::string& path) { + if (image.empty()) { + return; + } + std::ofstream stream(path, std::ios::binary); + if (stream.is_open()) { + stream.write(reinterpret_cast(image.data), image.total() * image.elemSize()); + } +} + +json clusterToJson(const MeasurementCluster& cluster, bool is_dynamic) { + json j{{"id", cluster.id}, {"is_dynamic", is_dynamic}, {"bounding_box", toJson(cluster.bounding_box)}}; + j["semantics"] = cluster.semantics ? toJson(*cluster.semantics) : json(nullptr); + return j; +} + +void saveClustersJson(const FrameData& data, const std::string& dir) { + saveRawInt32(data.object_image, dir + "/object_image.bin"); + saveRawInt32(data.dynamic_image, dir + "/dynamic_image.bin"); + + json clusters = json::array(); + for (const auto& cluster : data.semantic_clusters) { + clusters.push_back(clusterToJson(cluster, /*is_dynamic=*/false)); + } + for (const auto& cluster : data.dynamic_clusters) { + clusters.push_back(clusterToJson(cluster, /*is_dynamic=*/true)); + } + + std::ofstream stream(dir + "/clusters.json"); + if (stream.is_open()) { + stream << clusters.dump(2); + } +} + +// Parses the pose.txt format written by saveColorDepthPose: reads the trailing "Full +// transformation matrix (4x4)" block (the last 4 non-comment, non-empty lines). +std::optional loadPose(const std::string& dir) { + std::ifstream file(dir + "/pose.txt"); + if (!file.is_open()) { + LOG(ERROR) << "[FrameData] Missing pose.txt in " << dir; + return std::nullopt; + } + + std::vector> rows; + std::string line; + while (std::getline(file, line)) { + if (line.empty() || line[0] == '#') { + continue; + } + std::istringstream iss(line); + std::vector values((std::istream_iterator(iss)), std::istream_iterator()); + if (values.size() == 4) { + rows.push_back({values[0], values[1], values[2], values[3]}); + } + } + + if (rows.size() != 4) { + LOG(ERROR) << "[FrameData] Malformed pose.txt in " << dir + << " (expected a trailing 4x4 matrix block)"; + return std::nullopt; + } + + Eigen::Matrix4d matrix; + for (int r = 0; r < 4; ++r) { + for (int c = 0; c < 4; ++c) { + matrix(r, c) = rows[r][c]; + } + } + Eigen::Isometry3d pose; + pose.matrix() = matrix; + return pose; +} + +bool loadDepth(const std::string& dir, int width, int height, cv::Mat* depth) { + std::ifstream file(dir + "/depth.bin", std::ios::binary); + if (!file.is_open()) { + LOG(ERROR) << "[FrameData] Missing depth.bin in " << dir; + return false; + } + depth->create(height, width, CV_32FC1); + file.read(reinterpret_cast(depth->data), depth->total() * depth->elemSize()); + return static_cast(file) || file.eof(); +} + +// Not fatal if missing/unreadable: color is not needed for tracking itself (only depth/pose/ +// clusters are), only for optional visualization (e.g. overlay images); callers should treat an +// empty color_image as "not available" rather than failing the whole load. +cv::Mat loadColor(const std::string& dir) { + cv::Mat bgr_image = cv::imread(dir + "/color.png", cv::IMREAD_COLOR); + if (bgr_image.empty()) { + return {}; + } + cv::Mat rgb_image; + cv::cvtColor(bgr_image, rgb_image, cv::COLOR_BGR2RGB); + return rgb_image; +} + +bool loadRawInt32(const std::string& path, int width, int height, cv::Mat* out) { + std::ifstream file(path, std::ios::binary); + if (!file.is_open()) { + return false; + } + out->create(height, width, CV_32SC1); + file.read(reinterpret_cast(out->data), out->total() * out->elemSize()); + return static_cast(file) || file.eof(); +} + +std::unordered_map gatherPixelsById(const cv::Mat& image, + const cv::Mat& depth_image, + float min_range, + float max_range) { + std::unordered_map pixels_by_id; + if (image.empty()) { + return pixels_by_id; + } + for (int v = 0; v < image.rows; ++v) { + for (int u = 0; u < image.cols; ++u) { + const int32_t id = image.at(v, u); + if (id == 0) { + continue; + } + // Mirrors instance_forwarding.cpp's own pixel-validity gate (checks the raw depth/range + // value directly, not downstream-computed geometry): skip non-finite depth (no such guard + // exists anywhere in the production pipeline that builds cluster.pixels -- see + // tracker_debug.md bugfix writeup -- so this reconstruction filters explicitly to keep + // bbox/IoU computations well-defined regardless of pixel iteration order) and skip pixels + // outside [min_range, max_range], exactly matching instance_forwarding.cpp's own check. + const float depth = depth_image.at(v, u); + if (!std::isfinite(depth)) { + continue; + } + if (depth < min_range || (max_range > 0.f && depth > max_range)) { + continue; + } + pixels_by_id[id].emplace_back(u, v); + } + } + return pixels_by_id; +} + +void loadClustersJson(const std::string& dir, + int width, + int height, + float min_range, + float max_range, + FrameData* frame_data) { + std::ifstream clusters_file(dir + "/clusters.json"); + if (!clusters_file.is_open()) { + LOG(WARNING) << "[FrameData] Missing clusters.json in " << dir << "; treating as no detections."; + return; + } + json clusters_json; + clusters_file >> clusters_json; + + cv::Mat object_image; + cv::Mat dynamic_image; + if (loadRawInt32(dir + "/object_image.bin", width, height, &object_image)) { + frame_data->object_image = object_image; + } + if (loadRawInt32(dir + "/dynamic_image.bin", width, height, &dynamic_image)) { + frame_data->dynamic_image = dynamic_image; + } + + const auto& depth_image = frame_data->input.depth_image; + const auto object_pixels = gatherPixelsById(object_image, depth_image, min_range, max_range); + const auto dynamic_pixels = gatherPixelsById(dynamic_image, depth_image, min_range, max_range); + + for (const auto& entry : clusters_json) { + const bool is_dynamic = entry.at("is_dynamic").get(); + + MeasurementCluster cluster; + cluster.id = entry.at("id").get(); + cluster.bounding_box = boundingBoxFromJson(entry.at("bounding_box")); + if (!entry.at("semantics").is_null()) { + cluster.semantics = semanticsFromJson(entry.at("semantics")); + } + + const auto& pixels_by_id = is_dynamic ? dynamic_pixels : object_pixels; + const auto it = pixels_by_id.find(cluster.id); + if (it != pixels_by_id.end()) { + cluster.pixels = it->second; + } + + if (is_dynamic) { + frame_data->dynamic_clusters.push_back(cluster); + } else { + frame_data->semantic_clusters.push_back(cluster); + } + } +} + +} // namespace + +void FrameData::save(const std::string& observation_dir) const { + ensureDirectoryExists(observation_dir); + saveColorDepthPose(*this, observation_dir); + saveClustersJson(*this, observation_dir); +} + +FrameData::Ptr FrameData::load(const std::string& observation_dir, + const std::shared_ptr& camera, + TimeStamp stamp, + float min_range, + float max_range) { + if (!camera) { + LOG(ERROR) << "[FrameData] load() requires a valid camera."; + return nullptr; + } + + const auto pose = loadPose(observation_dir); + if (!pose) { + return nullptr; + } + + hydra::InputData input(camera); + input.timestamp_ns = stamp; + input.world_T_body = *pose; // valid since camera extrinsics are identity (loadCameraIntrinsics). + + input.color_image = loadColor(observation_dir); + + const auto& cam_config = camera->getConfig(); + if (!loadDepth(observation_dir, cam_config.width, cam_config.height, &input.depth_image)) { + return nullptr; + } + + if (!camera->finalizeRepresentations(input, /*force_world_frame=*/true)) { + LOG(ERROR) << "[FrameData] Camera::finalizeRepresentations failed for " << observation_dir; + return nullptr; + } + + auto frame_data = std::make_shared(input); + loadClustersJson( + observation_dir, cam_config.width, cam_config.height, min_range, max_range, frame_data.get()); + return frame_data; +} + +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 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(); + config.fy = j.at("fy").get(); + config.cx = j.at("cx").get(); + config.cy = j.at("cy").get(); + config.width = j.at("width").get(); + config.height = j.at("height").get(); + // 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(config, "reconstructed_camera"); +} + +} // namespace khronos diff --git a/khronos/src/active_window/data/track.cpp b/khronos/src/active_window/data/track.cpp index 60f65d8..d78b44c 100644 --- a/khronos/src/active_window/data/track.cpp +++ b/khronos/src/active_window/data/track.cpp @@ -37,7 +37,115 @@ #include "khronos/active_window/data/track.h" +#include + +#include +#include // Eigen adl_serializer (Vector3f, VectorXf, ...) + +#include "khronos/utils/json_utils.h" + namespace khronos { +namespace { + +using json = nlohmann::json; + +json toJson(const Observation& obs) { + return json{{"stamp", obs.stamp}, + {"semantic_cluster_id", obs.semantic_cluster_id}, + {"dynamic_cluster_id", obs.dynamic_cluster_id}}; +} + +Observation observationFromJson(const json& j) { + Observation obs; + obs.stamp = j.at("stamp").get(); + obs.semantic_cluster_id = j.at("semantic_cluster_id").get(); + obs.dynamic_cluster_id = j.at("dynamic_cluster_id").get(); + return obs; +} + +json toJson(const GlobalIndexSet& voxels) { + json arr = json::array(); + for (const GlobalIndex& voxel : voxels) { + arr.push_back({voxel.x(), voxel.y(), voxel.z()}); + } + return arr; +} + +GlobalIndexSet globalIndexSetFromJson(const json& j) { + GlobalIndexSet voxels; + for (const auto& entry : j) { + voxels.insert(GlobalIndex(entry.at(0).get(), entry.at(1).get(), entry.at(2).get())); + } + return voxels; +} + +json toJson(const Track& track) { + json j{{"id", track.id}, + {"last_seen", track.last_seen}, + {"first_seen", track.first_seen}, + {"last_bounding_box", khronos::toJson(track.last_bounding_box)}, + {"last_voxels", toJson(track.last_voxels)}, + {"last_points", track.last_points}, + {"last_voxel_size", track.last_voxel_size}, + {"last_centroid", track.last_centroid}, + {"num_features", track.num_features}, + {"is_dynamic", track.is_dynamic}, + {"confidence", track.confidence}}; + + j["observations"] = json::array(); + for (const Observation& obs : track.observations) { + j["observations"].push_back(toJson(obs)); + } + + j["semantics"] = track.semantics ? toJson(*track.semantics) : json(nullptr); + return j; +} + +Track trackFromJson(const json& j) { + Track track; + track.id = j.at("id").get(); + track.last_seen = j.at("last_seen").get(); + track.first_seen = j.at("first_seen").get(); + track.last_bounding_box = khronos::boundingBoxFromJson(j.at("last_bounding_box")); + track.last_voxels = globalIndexSetFromJson(j.at("last_voxels")); + track.last_points = j.at("last_points").get(); + track.last_voxel_size = j.at("last_voxel_size").get(); + track.last_centroid = j.at("last_centroid").get(); + track.num_features = j.at("num_features").get(); + track.is_dynamic = j.at("is_dynamic").get(); + track.confidence = j.at("confidence").get(); + + for (const auto& obs_json : j.at("observations")) { + track.observations.push_back(observationFromJson(obs_json)); + } + + if (!j.at("semantics").is_null()) { + track.semantics = khronos::semanticsFromJson(j.at("semantics")); + } + return track; +} + +} // namespace + +void Track::save(const std::string& filepath) const { + std::ofstream file(filepath); + if (!file.is_open()) { + LOG(ERROR) << "[Track] Failed to open '" << filepath << "' for writing."; + return; + } + file << toJson(*this).dump(2); +} + +Track Track::load(const std::string& filepath) { + std::ifstream file(filepath); + if (!file.is_open()) { + LOG(ERROR) << "[Track] Failed to open '" << filepath << "' for reading."; + return Track(); + } + json j; + file >> j; + return trackFromJson(j); +} void Track::updateSemantics(const std::optional& other) { if (!other) { diff --git a/khronos/src/active_window/motion_detection/free_space_motion_detector.cpp b/khronos/src/active_window/motion_detection/free_space_motion_detector.cpp index 7bd8a36..d5dbd2a 100644 --- a/khronos/src/active_window/motion_detection/free_space_motion_detector.cpp +++ b/khronos/src/active_window/motion_detection/free_space_motion_detector.cpp @@ -37,6 +37,7 @@ #include "khronos/active_window/motion_detection/free_space_motion_detector.h" +#include #include #include #include @@ -172,6 +173,10 @@ void FreeSpaceMotionDetector::setUpPointMapPart(const FrameData& data, } const auto& vertex = data.input.vertex_map.at(v, u); + if (!std::isfinite(vertex[0]) || !std::isfinite(vertex[1]) || !std::isfinite(vertex[2])) { + continue; + } + const Point p_W(vertex[0], vertex[1], vertex[2]); if (p_W.z() < min_z_world_) { continue; diff --git a/khronos/src/active_window/object_detection/instance_forwarding.cpp b/khronos/src/active_window/object_detection/instance_forwarding.cpp index 4e7bdbc..e89b63c 100644 --- a/khronos/src/active_window/object_detection/instance_forwarding.cpp +++ b/khronos/src/active_window/object_detection/instance_forwarding.cpp @@ -37,6 +37,7 @@ #include "khronos/active_window/object_detection/instance_forwarding.h" +#include #include #include @@ -105,6 +106,11 @@ void InstanceForwarding::extractSemanticClusters(FrameData& data) { } } + const auto& vertex = data.input.vertex_map.at(v, u); + if (!std::isfinite(vertex[0]) || !std::isfinite(vertex[1]) || !std::isfinite(vertex[2])) { + continue; + } + if (config.max_range > 0.f || config.min_range > 0.f) { const float range = data.input.range_image.at(v, u); if (range < config.min_range || (config.max_range > 0.f && range > config.max_range)) { diff --git a/khronos/src/active_window/object_extraction/mesh_object_extractor.cpp b/khronos/src/active_window/object_extraction/mesh_object_extractor.cpp index a6d069f..181e59e 100644 --- a/khronos/src/active_window/object_extraction/mesh_object_extractor.cpp +++ b/khronos/src/active_window/object_extraction/mesh_object_extractor.cpp @@ -62,6 +62,7 @@ void declare_config(MeshObjectExtractor::Config& config) { field(config.visualize_classification, "visualize_classification"); field(config.projective_integrator, "projective_integrator"); field(config.mesh_integrator, "mesh_integrator"); + enum_field(config.bbox_type, "bbox_type", std::vector{"aabb", "raabb"}); checkInRange( config.min_object_allocation_confidence, 0.0f, 1.0f, "min_object_allocation_confidence"); @@ -287,7 +288,10 @@ KhronosObjectAttributes::Ptr MeshObjectExtractor::extractStaticObject( if (object->mesh.points.empty()) { object->bounding_box = extent; } else { - object->bounding_box = BoundingBox(object->mesh.points); + const BoundingBox::Type bbox_type = config.bbox_type == Config::BBoxType::kRAABB + ? BoundingBox::Type::RAABB + : BoundingBox::Type::AABB; + object->bounding_box = BoundingBox(object->mesh.points, bbox_type); } if (object->bounding_box.volume() > config.max_object_volume) { CLOG(5) << "[MeshObjectExtractor] Dropping " << getTrackName(track) << ": large volume (" diff --git a/khronos/src/active_window/track_saver_sink.cpp b/khronos/src/active_window/track_saver_sink.cpp new file mode 100644 index 0000000..5bbf347 --- /dev/null +++ b/khronos/src/active_window/track_saver_sink.cpp @@ -0,0 +1,276 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos/active_window/track_saver_sink.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "khronos/active_window/data/frame_data.h" +#include "khronos/utils/output_file_utils.h" + +namespace khronos { +namespace { + +static const auto registration = config::RegistrationWithConfig( + "ActiveWindowTrackSaver"); + +} // namespace + +void declare_config(ActiveWindowTrackSaver::Config& config) { + using namespace config; + name("ActiveWindowTrackSaver"); + field(config.verbosity, "verbosity"); + field(config.output_directory, "output_directory"); + field(config.save_observations, "save_observations"); + field(config.save_masks, "save_masks"); + field(config.save_camera_intrinsics, "save_camera_intrinsics"); + field(config.save_pointcloud, "save_pointcloud"); + field(config.pointcloud_frame, "pointcloud_frame"); + field(config.save_track_json, "save_track_json"); + field(config.save_track_symlinks, "save_track_symlinks"); + checkCondition(!config.output_directory.empty(), "output_directory must not be empty"); + checkCondition(config.pointcloud_frame == "world" || config.pointcloud_frame == "robot" || + config.pointcloud_frame == "body" || config.pointcloud_frame == "sensor", + "pointcloud_frame must be one of 'world', 'robot'/'body', or 'sensor'"); +} + +ActiveWindowTrackSaver::ActiveWindowTrackSaver(const Config& config) + : config(config::checkValid(config)) { + MLOG(1) << "[ActiveWindowTrackSaver] Initialized, output_directory: " << config.output_directory; +} + +std::string ActiveWindowTrackSaver::getBaseDir() const { + if (base_dir_.empty()) { + const auto now = std::chrono::system_clock::now(); + const auto time_t_now = std::chrono::system_clock::to_time_t(now); + std::stringstream ss; + ss << std::put_time(std::localtime(&time_t_now), "%Y%m%d_%H%M%S"); + base_dir_ = config.output_directory + "/khronos_tracks_" + ss.str(); + ensureDirectoryExists(base_dir_); + MLOG(1) << "[ActiveWindowTrackSaver] Saving track data to: " << base_dir_; + } + return base_dir_; +} + +std::string ActiveWindowTrackSaver::getTrackDir(int track_id) const { + const std::string track_dir = getBaseDir() + "/tracks/track_" + std::to_string(track_id); + ensureDirectoryExists(track_dir); + return track_dir; +} + +void ActiveWindowTrackSaver::saveMask(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + int semantic_cluster_id, + cv::Mat* binary_mask_out) const { + if (semantic_cluster_id < 0 || data.object_image.empty()) { + return; + } + const cv::Mat binary_mask = (data.object_image == semantic_cluster_id); + cv::imwrite(track_dir + "/mask_" + ts_str + ".png", binary_mask); + if (binary_mask_out) { + *binary_mask_out = binary_mask; + } +} + +void ActiveWindowTrackSaver::saveOverlay(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + const cv::Mat* binary_mask) const { + if (data.input.color_image.empty() || !binary_mask || binary_mask->empty()) { + return; + } + cv::Mat bgr_image; + cv::cvtColor(data.input.color_image, bgr_image, cv::COLOR_RGB2BGR); + + cv::Mat overlay = bgr_image.clone(); + overlay.setTo(cv::Scalar(0, 0, 255), *binary_mask); // Red in BGR. + cv::Mat blended; + cv::addWeighted(bgr_image, 0.6, overlay, 0.4, 0, blended); + cv::imwrite(track_dir + "/overlay_" + ts_str + ".png", blended); +} + +void ActiveWindowTrackSaver::savePointcloud(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + const cv::Mat* binary_mask) const { + const cv::Mat& vertex_map = data.input.vertex_map; + if (vertex_map.empty()) { + return; + } + const bool has_color = !data.input.color_image.empty() && + data.input.color_image.size() == vertex_map.size(); + const bool has_mask = binary_mask && !binary_mask->empty() && + binary_mask->size() == vertex_map.size(); + + // vertex_map is always in world/map frame (ActiveWindow::createData requests + // vertices_in_world_frame=true). Reference-frame transform to express saved points relative to + // the requested frame instead, so e.g. "robot" makes spatially-coincident tracks show up near + // the origin regardless of where the robot was in the world. + Eigen::Isometry3f frame_T_world = Eigen::Isometry3f::Identity(); + if (config.pointcloud_frame == "robot" || config.pointcloud_frame == "body") { + frame_T_world = data.input.world_T_body.inverse().cast(); + } else if (config.pointcloud_frame == "sensor") { + frame_T_world = data.input.getSensorPose().inverse().cast(); + } + + std::vector points; + std::vector colors; + points.reserve(vertex_map.total()); + for (int r = 0; r < vertex_map.rows; ++r) { + for (int c = 0; c < vertex_map.cols; ++c) { + if (has_mask && (*binary_mask).at(r, c) == 0) { + continue; + } + const cv::Vec3f point = vertex_map.at(r, c); + if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) { + continue; + } + const Eigen::Vector3f point_in_frame = + frame_T_world * Eigen::Vector3f(point[0], point[1], point[2]); + points.push_back(cv::Vec3f(point_in_frame.x(), point_in_frame.y(), point_in_frame.z())); + colors.push_back(has_color ? data.input.color_image.at(r, c) + : cv::Vec3b(255, 255, 255)); + } + } + if (points.empty()) { + return; + } + + std::ofstream ply_stream(track_dir + "/cloud_" + ts_str + ".ply"); + if (!ply_stream.is_open()) { + return; + } + ply_stream << "ply\nformat ascii 1.0\n" + << "element vertex " << points.size() << "\n" + << "property float x\nproperty float y\nproperty float z\n" + << "property uchar red\nproperty uchar green\nproperty uchar blue\n" + << "end_header\n"; + ply_stream << std::fixed << std::setprecision(6); + for (size_t i = 0; i < points.size(); ++i) { + // color_image is RGB order; write as-is (r, g, b). + ply_stream << points[i][0] << " " << points[i][1] << " " << points[i][2] << " " + << static_cast(colors[i][0]) << " " << static_cast(colors[i][1]) << " " + << static_cast(colors[i][2]) << "\n"; + } +} + +void ActiveWindowTrackSaver::saveTrackJson(const std::string& track_dir, const Track& track) const { + // Delegates to Track::save so track.json is a full, reconstructable serialization (see + // khronos/active_window/data/track.h) rather than a lossy summary -- this is the exact input + // the offline tracker-replay harness needs. + track.save(track_dir + "/track.json"); +} + +void ActiveWindowTrackSaver::createTrackSymlinks(const std::string& track_dir, + const std::string& ts_str) const { + namespace fs = std::filesystem; + const std::string observation_rel = "../../observations/" + ts_str; + const std::pair links[] = { + {"color_" + ts_str + ".png", observation_rel + "/color.png"}, + {"depth_" + ts_str + ".png", observation_rel + "/depth.png"}, + {"depth_" + ts_str + ".bin", observation_rel + "/depth.bin"}, + {"pose_" + ts_str + ".txt", observation_rel + "/pose.txt"}, + }; + for (const auto& [link_name, target] : links) { + const fs::path link_path = fs::path(track_dir) / link_name; + std::error_code ec; + if (fs::exists(fs::symlink_status(link_path))) { + continue; // Already created for this observation (e.g. sink restarted mid-run). + } + fs::create_symlink(target, link_path, ec); + if (ec) { + LOG(WARNING) << "[ActiveWindowTrackSaver] Failed to symlink " << link_path << " -> " << target + << ": " << ec.message(); + } + } +} + +void ActiveWindowTrackSaver::call(const FrameData& data, + const VolumetricMap& /*map*/, + const Tracks& tracks) const { + const std::string ts_str = std::to_string(data.input.timestamp_ns); + + if (config.save_observations) { + data.save(getBaseDir() + "/observations/" + ts_str); + } + if (config.save_camera_intrinsics && !intrinsics_saved_) { + const auto* camera = dynamic_cast(&data.input.getSensor()); + if (camera) { + saveCameraIntrinsics(*camera, getBaseDir()); + intrinsics_saved_ = true; + } + } + + for (const auto& track : tracks) { + // Only save data for tracks observed in this exact frame; the sink only has access to the + // current frame, so past observations cannot be backfilled here. + if (track.observations.empty() || track.last_seen != data.input.timestamp_ns) { + continue; + } + const Observation& obs = track.observations.back(); + const std::string track_dir = getTrackDir(track.id); + + cv::Mat binary_mask; + if (config.save_masks) { + saveMask(track_dir, ts_str, data, obs.semantic_cluster_id, &binary_mask); + saveOverlay(track_dir, ts_str, data, binary_mask.empty() ? nullptr : &binary_mask); + } + if (config.save_pointcloud) { + savePointcloud(track_dir, ts_str, data, binary_mask.empty() ? nullptr : &binary_mask); + } + if (config.save_track_json) { + saveTrackJson(track_dir, track); + } + if (config.save_track_symlinks) { + createTrackSymlinks(track_dir, ts_str); + } + } +} + +} // namespace khronos diff --git a/khronos/src/active_window/tracking/max_iou_tracker.cpp b/khronos/src/active_window/tracking/max_iou_tracker.cpp index 45bf3db..f9de70a 100644 --- a/khronos/src/active_window/tracking/max_iou_tracker.cpp +++ b/khronos/src/active_window/tracking/max_iou_tracker.cpp @@ -72,6 +72,8 @@ struct MatchResult { } const status; const std::optional similiarity = std::nullopt; + const std::optional lhs_category = std::nullopt; + const std::optional rhs_category = std::nullopt; }; std::ostream& operator<<(std::ostream& out, const MatchResult& result) { @@ -80,7 +82,8 @@ std::ostream& operator<<(std::ostream& out, const MatchResult& result) { out << "no match (invalid semantics)"; break; case MatchResult::Status::kMismatchedCategories: - out << "no match (categories are different)"; + out << "no match (categories are different: " << result.lhs_category.value_or(-1) << " vs " + << result.rhs_category.value_or(-1) << ")"; break; case MatchResult::Status::kMismatchedFeatures: out << "no match (feature dimensions disagree)"; @@ -110,7 +113,10 @@ MatchResult semanticsMatch(const std::optional& lhs, // For openset cases, all objects have the same (unknown) semantic ID. if (lhs->category_id != rhs->category_id) { - return {MatchResult::Status::kMismatchedCategories}; + return {MatchResult::Status::kMismatchedCategories, + std::nullopt, + lhs->category_id, + rhs->category_id}; } if (lhs->feature.size() != rhs->feature.size()) { @@ -139,12 +145,14 @@ void declare_config(MaxIoUTracker::Config& config) { enum_field(config.semantic_association, "semantic_association", std::vector{"assign_cluster", "assign_track"}); + enum_field(config.bbox_type, "bbox_type", std::vector{"aabb", "raabb"}); field(config.min_semantic_iou, "min_semantic_iou"); field(config.min_cosine_sim, "min_cosine_sim"); field(config.min_cross_iou, "min_cross_iou"); field(config.max_dynamic_distance, "max_dynamic_distance", "m"); field(config.min_num_observations, "min_num_observations", "frames"); field(config.voxel_size, "voxel_size", "m"); + checkInRange(config.min_cross_iou, 0.0f, 1.0f, "min_cross_iou"); checkInRange(config.min_semantic_iou, 0.0f, 1.0f, "min_semantic_iou"); @@ -430,17 +438,20 @@ void MaxIoUTracker::assignStaticTracksToCluster(const FrameData& data, } void MaxIoUTracker::setupTrackMeasurements(FrameData& data) const { + const BoundingBox::Type bbox_type = config.bbox_type == Config::BBoxType::kRAABB + ? BoundingBox::Type::RAABB + : BoundingBox::Type::AABB; for (auto& cluster : data.semantic_clusters) { setupTrackMeasurement(data, cluster); // NOTE(lschmid): Compute the bounding boxes for all clusters for visualization and extent // computation in the future. cluster.bounding_box = - BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map)); + BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map), bbox_type); } for (auto& cluster : data.dynamic_clusters) { setupTrackMeasurement(data, cluster); cluster.bounding_box = - BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map)); + BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map), bbox_type); } } @@ -552,20 +563,25 @@ float MaxIoUTracker::computeIoUVoxels(const FrameData& /* data */, return intersection / (cluster.voxels.size() + track.last_voxels.size() - intersection); } -float MaxIoUTracker::computeIoUPixels(const FrameData& data, - const MeasurementCluster& cluster, - const Track& track) const { - // Project every pixel of cluster 1 into the frame of cluster 2. +std::set MaxIoUTracker::reprojectPoints(const FrameData& data, const Points& points) const { const Transform sensor_T_world = data.input.getSensorPose().inverse(); const Sensor& sensor = data.input.getSensor(); std::set reprojected_pixels; - for (const Point& point : track.last_points) { + for (const Point& point : points) { int u, v; const auto p_sensor = sensor_T_world * Eigen::Vector3d(point[0], point[1], point[2]); if (sensor.projectPointToImagePlane(p_sensor.cast(), u, v)) { reprojected_pixels.emplace(u, v); } } + return reprojected_pixels; +} + +float MaxIoUTracker::computeIoUPixels(const FrameData& data, + const MeasurementCluster& cluster, + const Track& track) const { + // Project every pixel of cluster 1 into the frame of cluster 2. + const auto reprojected_pixels = reprojectPoints(data, track.last_points); // Compute IoU of the reprojected pixels and cluster 2. float intersection = 0.f; diff --git a/khronos/src/utils/icp_registration_utils.cpp b/khronos/src/utils/icp_registration_utils.cpp new file mode 100644 index 0000000..f6f880c --- /dev/null +++ b/khronos/src/utils/icp_registration_utils.cpp @@ -0,0 +1,71 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos/utils/icp_registration_utils.h" + +#include + +namespace khronos { + +using Cloud = std::vector; +using Result = ICPRegistrationUtils::Result; + +Result ICPRegistrationUtils::registerPointClouds(const Cloud& p_source, + const Cloud& p_target, + size_t num_threads, + float downsampling_resolution, + float max_correspondence_distance) { + small_gicp::RegistrationSetting setting; + setting.num_threads = num_threads; + setting.downsampling_resolution = downsampling_resolution; + setting.max_correspondence_distance = max_correspondence_distance; + + // Note: small_gicp::align expects (target, source, initial_guess) + // This will return T_target_source + auto result = small_gicp::align(p_target, p_source, Eigen::Isometry3d::Identity(), setting); + return { + result.T_target_source, + result.converged, + result.iterations, + result.num_inliers, + result.H, + result.b, + result.error, + }; +} + +} // namespace khronos diff --git a/khronos/src/utils/json_utils.cpp b/khronos/src/utils/json_utils.cpp new file mode 100644 index 0000000..a852e07 --- /dev/null +++ b/khronos/src/utils/json_utils.cpp @@ -0,0 +1,76 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos/utils/json_utils.h" + +#include // Eigen adl_serializer (Vector3f, VectorXf, ...) + +namespace khronos { + +nlohmann::json toJson(const BoundingBox& bbox) { + return nlohmann::json{{"type", static_cast(bbox.type)}, + {"dimensions", bbox.dimensions}, + {"world_P_center", bbox.world_P_center}, + {"world_R_center", Eigen::Quaternionf(bbox.world_R_center)}}; +} + +BoundingBox boundingBoxFromJson(const nlohmann::json& j) { + BoundingBox bbox; + bbox.type = static_cast(j.at("type").get()); + bbox.dimensions = j.at("dimensions").get(); + bbox.world_P_center = j.at("world_P_center").get(); + bbox.world_R_center = j.at("world_R_center").get().toRotationMatrix(); + return bbox; +} + +nlohmann::json toJson(const SemanticClusterInfo& semantics) { + nlohmann::json j{{"category_id", semantics.category_id}}; + if (semantics.feature.size() > 1) { + j["feature"] = semantics.feature; + } + return j; +} + +SemanticClusterInfo semanticsFromJson(const nlohmann::json& j) { + SemanticClusterInfo semantics(j.at("category_id").get()); + if (j.contains("feature")) { + semantics.feature = j.at("feature").get(); + } + return semantics; +} + +} // namespace khronos diff --git a/khronos_msgs/CMakeLists.txt b/khronos_msgs/CMakeLists.txt index 70b15fd..3485ad4 100644 --- a/khronos_msgs/CMakeLists.txt +++ b/khronos_msgs/CMakeLists.txt @@ -6,6 +6,7 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) @@ -18,11 +19,14 @@ rosidl_generate_interfaces( msg/Changes.msg msg/ObjectChange.msg msg/SpatioTemporalVisualizerState.msg + msg/ChangedObjectInfo.msg + msg/AwcdChanges.msg srv/SpatioTemporalVisualizerSetState.srv srv/SpatioTemporalVisualizerSetTimeMode.srv srv/SpatioTemporalVisualizerSetup.srv DEPENDENCIES builtin_interfaces + geometry_msgs std_msgs ) diff --git a/khronos_msgs/msg/AwcdChanges.msg b/khronos_msgs/msg/AwcdChanges.msg new file mode 100644 index 0000000..b8b1806 --- /dev/null +++ b/khronos_msgs/msg/AwcdChanges.msg @@ -0,0 +1,14 @@ +# Batch of object changes detected by the ActiveWindowChangeDetector in a single frame, +# published by the ActiveWindowChangeDetectorPublisher sink for the base station to consume. + +# header.stamp = publish time. header.frame_id = frame the attributes below are reported in. +std_msgs/Header header + +# Name of the robot that produced this report. +string robot_name + +# Objects present in the prior map but detected as removed this frame. +khronos_msgs/ChangedObjectInfo[] removed_objects + +# Objects newly detected and not present in the prior map. +khronos_msgs/ChangedObjectInfo[] added_objects diff --git a/khronos_msgs/msg/ChangedObjectInfo.msg b/khronos_msgs/msg/ChangedObjectInfo.msg new file mode 100644 index 0000000..37759ea --- /dev/null +++ b/khronos_msgs/msg/ChangedObjectInfo.msg @@ -0,0 +1,23 @@ +# Info describing a single changed object (removed or newly added), reported by the +# ActiveWindowChangeDetectorPublisher sink. + +# Identifier: prior-DSG NodeId for removed objects, Track id for added objects. +int64 id + +# Time the change occurred (added -> Track::first_seen, removed -> detection/publish time). +builtin_interfaces/Time stamp + +# --- Attributes below are populated for ADDED objects only; left default/zero for removed. --- + +# Object centroid in the report frame (see AwcdChanges.msg header.frame_id). +geometry_msgs/Point centroid + +# Oriented bounding box, in the report frame. +geometry_msgs/Point bbox_center +geometry_msgs/Vector3 bbox_dimensions +geometry_msgs/Quaternion bbox_orientation + +# Semantics. +int32 semantic_label +string name +float32 confidence diff --git a/khronos_msgs/package.xml b/khronos_msgs/package.xml index 8e34125..97f87ab 100644 --- a/khronos_msgs/package.xml +++ b/khronos_msgs/package.xml @@ -17,6 +17,7 @@ ament_cmake rosidl_default_generators builtin_interfaces + geometry_msgs std_msgs rosidl_default_runtime diff --git a/khronos_ros/CMakeLists.txt b/khronos_ros/CMakeLists.txt index d1d6329..3840d73 100644 --- a/khronos_ros/CMakeLists.txt +++ b/khronos_ros/CMakeLists.txt @@ -17,6 +17,8 @@ find_package(gflags REQUIRED) find_package(hydra_ros REQUIRED) find_package(khronos REQUIRED) find_package(khronos_msgs REQUIRED) +find_package(roman_msgs REQUIRED) +find_package(tf2_ros REQUIRED) # ############################################################################## # Libraries # @@ -29,7 +31,11 @@ add_library( src/experiments/experiment_manager.cpp src/experiments/experiment_directory.cpp src/khronos_pipeline.cpp + src/active_window/tf_transformation_getter.cpp + src/active_window/tf_icp_publisher.cpp + src/active_window/active_window_change_detector_publisher.cpp src/visualization/active_window_visualizer.cpp + src/visualization/active_window_change_detector_visualizer.cpp src/visualization/visualization_utils.cpp src/visualization/spatio_temporal_visualizer.cpp) target_include_directories( @@ -38,7 +44,7 @@ target_include_directories( "$" ) target_link_libraries(${PROJECT_NAME} PUBLIC khronos::khronos hydra_ros::hydra_ros PRIVATE cv_bridge::cv_bridge) -ament_target_dependencies(${PROJECT_NAME} PUBLIC khronos_msgs) +ament_target_dependencies(${PROJECT_NAME} PUBLIC khronos_msgs roman_msgs tf2_ros) # ############################################################################## # Executables # @@ -68,5 +74,5 @@ install(PROGRAMS ) ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET) -ament_export_dependencies(gflags hydra_ros khronos khronos_msgs) +ament_export_dependencies(gflags hydra_ros khronos khronos_msgs roman_msgs tf2_ros) ament_package() diff --git a/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_publisher.h b/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_publisher.h new file mode 100644 index 0000000..faa89c3 --- /dev/null +++ b/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_publisher.h @@ -0,0 +1,112 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "khronos/active_window/change_detection/active_window_change_detector.h" +#include "khronos/active_window/data/track.h" + +namespace khronos { + +/** + * @brief ActiveWindowCDSink that packs the detector's per-frame removed/added object + * results into a khronos_msgs/AwcdChanges message and publishes it, so that other + * processes (e.g. a base station) can consume the change-detection output. + */ +class ActiveWindowChangeDetectorPublisher : public ActiveWindowChangeDetector::ActiveWindowCDSink { + public: + struct Config : hydra::VerbosityConfig { + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + + //! Frame in which reported object attributes (centroid, bbox) are expressed. + std::string global_frame_name = hydra::GlobalInfo::instance().getFrames().map; + + //! Name of the robot reporting the changes, forwarded into AwcdChanges::robot_name. + std::string robot_name; + + //! Topic to publish AwcdChanges messages on. + std::string topic = "awcd_changes"; + + //! Publisher queue size. + int queue_size = 10; + } const config; + + explicit ActiveWindowChangeDetectorPublisher(const Config& config, + const ianvs::NodeHandle* nh = nullptr); + virtual ~ActiveWindowChangeDetectorPublisher() = default; + + // KhronosSink callback - called each frame. Only publishes when the set of removed and/or + // added object ids differs from the last published sets (see last_removed_ids_/last_added_ids_). + void call(const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_objects, + const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const override; + + private: + //! Fills in a ChangedObjectInfo entry for a removed object (id + latched first-removed time). + khronos_msgs::msg::ChangedObjectInfo makeRemovedInfo( + const ActiveWindowChangeDetector::RemovedObject& obj) const; + + //! Fills in a ChangedObjectInfo entry (full attributes) for a newly-added track. + khronos_msgs::msg::ChangedObjectInfo makeAddedInfo(const Track& track, + const Eigen::Isometry3d& prior_T_current) const; + + // ROS + ianvs::NodeHandle nh_; + rclcpp::Publisher::SharedPtr changes_pub_; + + // Set-membership of the last *published* removed/added ids, used to gate publishing to only + // when the reported set of changes actually changed (rather than every frame). + mutable std::optional> last_removed_ids_; + mutable std::optional> last_added_ids_; +}; + +void declare_config(ActiveWindowChangeDetectorPublisher::Config& config); + +} // namespace khronos diff --git a/khronos_ros/include/khronos_ros/active_window/tf_icp_publisher.h b/khronos_ros/include/khronos_ros/active_window/tf_icp_publisher.h new file mode 100644 index 0000000..5311e65 --- /dev/null +++ b/khronos_ros/include/khronos_ros/active_window/tf_icp_publisher.h @@ -0,0 +1,99 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "khronos/active_window/change_detection/active_window_change_detector.h" +#include "khronos/active_window/data/track.h" + +namespace khronos { + +/** + * @brief ActiveWindowCDSink that publishes the ICP refinement delta as a TF. + * + * Publishes pre_icp_odom → odom (StaticTF), splitting the full odom_T_map + * transform (received as current_T_prior from AWCD) into: + * - map → pre_icp_odom: published externally by hydra_multi (ROMAN result) + * - pre_icp_odom → odom: published here (ICP delta on top of ROMAN) + * + * On startup, seeds identity so the TF chain map → odom is immediately resolvable. + * When enable_icp_refinement=false in AWCD, current_T_prior equals the ROMAN result + * and the ICP delta is identity. + */ +class TfIcpPublisher : public ActiveWindowChangeDetector::ActiveWindowCDSink { + public: + struct Config : hydra::VerbosityConfig { + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + + //! Frame published by hydra_multi as the ROMAN-only odom estimate (child of map). + std::string pre_icp_odom_frame = ""; + //! Robot odometry frame (child of pre_icp_odom_frame). Empty = GlobalInfo odom frame. + std::string odom_frame = ""; + } const config; + + explicit TfIcpPublisher(const Config& config, const ianvs::NodeHandle* nh = nullptr); + virtual ~TfIcpPublisher() = default; + + void call(const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_objects, + const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const override; + + private: + std::string getOdomFrame() const; + void broadcastTransform(const Eigen::Isometry3d& pre_icp_odom_T_odom) const; + + ianvs::NodeHandle nh_; + std::unique_ptr tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +void declare_config(TfIcpPublisher::Config& config); + +} // namespace khronos diff --git a/khronos_ros/include/khronos_ros/active_window/tf_transformation_getter.h b/khronos_ros/include/khronos_ros/active_window/tf_transformation_getter.h new file mode 100644 index 0000000..5de5823 --- /dev/null +++ b/khronos_ros/include/khronos_ros/active_window/tf_transformation_getter.h @@ -0,0 +1,94 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include +#include + +#include +#include + +#include "khronos/active_window/change_detection/transformation_getter.h" + +namespace khronos { + +/** + * @brief TransformationGetter that obtains odom_T_prior by querying TF2. + * + * Looks up `prior_frame_id` → `robot_frame_id` (odom frame) via tf_buffer each + * call, computes odom_T_prior = inverse(map_T_odom), and applies a change- + * threshold filter: returns nullopt when the translation change since the last + * reported transform is below tf_change_threshold_m. + */ +class TFTransformationGetter : public TransformationGetter { + public: + struct Config : hydra::VerbosityConfig { + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + + //! Frame ID of the prior map (hydra_multi world_frame, typically "map"). + std::string prior_frame_id = "map"; + //! Robot odometry frame published by hydra_multi. + //! If empty, falls back to hydra::GlobalInfo::instance().getFrames().odom. + std::string robot_frame_id = ""; + //! Minimum translation change [m] before reporting a new transform. + double tf_change_threshold_m = 0.05; + } const config; + + explicit TFTransformationGetter(const Config& config); + + /** + * @brief Look up map→odom TF and return odom_T_map = current_T_prior. + * Returns nullopt on TF failure or if change is below threshold. + */ + std::optional getTransformation() const override; + + private: + //! Returns robot_frame_id from config if set, otherwise GlobalInfo odom frame. + std::string getRobotFrame() const; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + mutable Eigen::Isometry3d last_reported_ = Eigen::Isometry3d::Identity(); + mutable bool has_last_ = false; +}; + +void declare_config(TFTransformationGetter::Config& config); + +} // namespace khronos diff --git a/khronos_ros/include/khronos_ros/visualization/active_window_change_detector_visualizer.h b/khronos_ros/include/khronos_ros/visualization/active_window_change_detector_visualizer.h new file mode 100644 index 0000000..475542f --- /dev/null +++ b/khronos_ros/include/khronos_ros/visualization/active_window_change_detector_visualizer.h @@ -0,0 +1,133 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "khronos/active_window/change_detection/active_window_change_detector.h" +#include "khronos/active_window/data/track.h" + + +namespace khronos { + +class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector::ActiveWindowCDSink { + public: + struct Config : hydra::VerbosityConfig { + // TODO(multy): after hydra is updated, should change it to also include a prefix like: "[Active + // Window Change Detector Visualizer] " + // int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} + + //! Frame in which to publish visualizations. + std::string global_frame_name = hydra::GlobalInfo::instance().getFrames().map; + // TODO(multy): this should be default to some awcd dedicated frame? Not the global map frame, + // The transformation between the global map frame and the awcd frame can be identity to + // indicates no difference + + //! Scene graph renderer config. + hydra::SceneGraphRenderer::Config renderer; + + //! Mesh plugin config (optional - if coloring is not set, uses mesh colors). + hydra::MeshPlugin::Config mesh; + + //! Publisher queue sizes. + int queue_size = 10; + + //! Width in meters of lines indicating bounding boxes. + float bounding_box_line_width = 0.1f; + + //! Minimum time between visualization redraws (seconds). 0 = draw every frame. + double min_draw_period_s = 0.0; + } const config; + + explicit ActiveWindowChangeDetectorVisualizer(const Config& config, + const ianvs::NodeHandle* nh = nullptr); + virtual ~ActiveWindowChangeDetectorVisualizer() = default; + + // KhronosSink callback - called each frame. + void call(const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_objects, + const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const override; + + private: + void drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const; + + void visualizeChangedObjects( + const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_objects) const; + + void visualizeAddedObjects(const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const; + + // ROS + ianvs::NodeHandle nh_; + rclcpp::Publisher::SharedPtr object_bbox_pub_; + // Renderer and plugins + std::shared_ptr renderer_; + std::shared_ptr mesh_plugin_; + + // Variables + mutable bool has_drawn_ = false; + mutable rclcpp::Time stamp_; + mutable bool stamp_is_set_ = false; + mutable std::optional last_draw_time_; + mutable hydra::MarkerTracker object_bbox_tracker_; + mutable hydra::MarkerTracker added_object_bbox_tracker_; + + // Time stamp caching for synchronization of multiple visualizations. + rclcpp::Time getStamp() const { return stamp_is_set_ ? stamp_ : nh_.now(); } +}; + +void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config); + +} // namespace khronos diff --git a/khronos_ros/include/khronos_ros/visualization/active_window_visualizer.h b/khronos_ros/include/khronos_ros/visualization/active_window_visualizer.h index b02f5cd..6594fcd 100644 --- a/khronos_ros/include/khronos_ros/visualization/active_window_visualizer.h +++ b/khronos_ros/include/khronos_ros/visualization/active_window_visualizer.h @@ -100,6 +100,15 @@ class ActiveWindowVisualizer : public ActiveWindow::KhronosSink { // Map of configs for display sensor information. hydra::SensorMap::Config sensor_displays; + + // Control the marker array opacity + uint8_t marker_opacity = 255; + + // Number of slice to visualize + int num_slices = 1; + + // slice distance + float slice_distance_multiplier = 1.0f; } const config; // Construction. diff --git a/khronos_ros/package.xml b/khronos_ros/package.xml index e317a4c..b00d077 100644 --- a/khronos_ros/package.xml +++ b/khronos_ros/package.xml @@ -22,6 +22,7 @@ khronos_msgs libgflags-dev rclcpp + roman_msgs std_srvs tf2_ros diff --git a/khronos_ros/src/active_window/active_window_change_detector_publisher.cpp b/khronos_ros/src/active_window/active_window_change_detector_publisher.cpp new file mode 100644 index 0000000..fde0de1 --- /dev/null +++ b/khronos_ros/src/active_window/active_window_change_detector_publisher.cpp @@ -0,0 +1,172 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos_ros/active_window/active_window_change_detector_publisher.h" + +#include +#include +#include + +namespace khronos { +namespace { + +static const auto registration = + config::RegistrationWithConfig( + "ActiveWindowChangeDetectorPublisher"); + +} + +void declare_config(ActiveWindowChangeDetectorPublisher::Config& config) { + using namespace config; + name("ActiveWindowChangeDetectorPublisher"); + field(config.verbosity, "verbosity"); + field(config.global_frame_name, "global_frame_name"); + field(config.robot_name, "robot_name"); + field(config.topic, "topic"); + field(config.queue_size, "queue_size"); +} + +ActiveWindowChangeDetectorPublisher::ActiveWindowChangeDetectorPublisher( + const Config& config, + const ianvs::NodeHandle* nh) + : config(config::checkValid(config)), + nh_(nh ? *nh / "change_detector_publisher" + : ianvs::NodeHandle::this_node("change_detector_publisher")) { + changes_pub_ = nh_.create_publisher(config.topic, config.queue_size); + MLOG(1) << "[ActiveWindowChangeDetectorPublisher] Initialized."; +} + +void ActiveWindowChangeDetectorPublisher::call( + const DynamicSceneGraph::Ptr& /* dsg */, + const std::vector& removed_objects, + const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const { + // Gate publishing on set-membership change: only publish if the set of removed and/or added + // object ids differs from the last message actually published (not just from last frame's + // detector output), so an unchanged scene does not keep re-publishing every frame. + std::set cur_removed_ids; + for (const auto& obj : removed_objects) { + cur_removed_ids.insert(static_cast(obj.id)); + } + std::set cur_added_ids; + for (const Track& track : newly_added_tracks) { + cur_added_ids.insert(static_cast(track.id)); + } + + if (last_removed_ids_.has_value() && last_added_ids_.has_value() && + *last_removed_ids_ == cur_removed_ids && *last_added_ids_ == cur_added_ids) { + return; + } + + khronos_msgs::msg::AwcdChanges msg; + msg.header.stamp = nh_.now(); + msg.header.frame_id = config.global_frame_name; + msg.robot_name = config.robot_name; + + msg.removed_objects.reserve(removed_objects.size()); + for (const auto& obj : removed_objects) { + msg.removed_objects.push_back(makeRemovedInfo(obj)); + } + + // Track bounding boxes / centroids are stored in the current (odom) frame; transform + // them into the prior/report frame before publishing, mirroring the visualizer. + const Eigen::Isometry3d prior_T_current = current_T_prior.inverse(); + msg.added_objects.reserve(newly_added_tracks.size()); + for (const Track& track : newly_added_tracks) { + msg.added_objects.push_back(makeAddedInfo(track, prior_T_current)); + } + + changes_pub_->publish(msg); + last_removed_ids_ = std::move(cur_removed_ids); + last_added_ids_ = std::move(cur_added_ids); +} + +khronos_msgs::msg::ChangedObjectInfo ActiveWindowChangeDetectorPublisher::makeRemovedInfo( + const ActiveWindowChangeDetector::RemovedObject& obj) const { + khronos_msgs::msg::ChangedObjectInfo info; + info.id = static_cast(obj.id); + // Latched at the detector: constant across every message that reports this object as removed. + info.stamp = static_cast( + rclcpp::Time(static_cast(obj.first_removed_ns))); + return info; +} + +khronos_msgs::msg::ChangedObjectInfo ActiveWindowChangeDetectorPublisher::makeAddedInfo( + const Track& track, + const Eigen::Isometry3d& prior_T_current) const { + khronos_msgs::msg::ChangedObjectInfo info; + info.id = static_cast(track.id); + info.stamp = static_cast( + rclcpp::Time(static_cast(track.first_seen))); + + const Eigen::Vector3d centroid = prior_T_current * track.last_centroid.cast(); + info.centroid.x = centroid.x(); + info.centroid.y = centroid.y(); + info.centroid.z = centroid.z(); + + BoundingBox bbox = track.last_bounding_box; + if (bbox.isValid()) { + bbox.transform(prior_T_current); + info.bbox_center.x = bbox.world_P_center.x(); + info.bbox_center.y = bbox.world_P_center.y(); + info.bbox_center.z = bbox.world_P_center.z(); + info.bbox_dimensions.x = bbox.dimensions.x(); + info.bbox_dimensions.y = bbox.dimensions.y(); + info.bbox_dimensions.z = bbox.dimensions.z(); + const Eigen::Quaternionf q(bbox.world_R_center); + info.bbox_orientation.w = q.w(); + info.bbox_orientation.x = q.x(); + info.bbox_orientation.y = q.y(); + info.bbox_orientation.z = q.z(); + } + + if (track.semantics.has_value()) { + info.semantic_label = track.semantics->category_id; + } else { + info.semantic_label = -1; + } + // NOTE(multy): Track only carries a numeric category_id; no label-space name lookup is + // wired in here, so `name` is left empty. The base station can resolve it against the + // same label space config (instance_seg_label_space.yaml) used to produce category_id. + info.confidence = track.confidence; + + return info; +} + +} // namespace khronos diff --git a/khronos_ros/src/active_window/tf_icp_publisher.cpp b/khronos_ros/src/active_window/tf_icp_publisher.cpp new file mode 100644 index 0000000..dd44885 --- /dev/null +++ b/khronos_ros/src/active_window/tf_icp_publisher.cpp @@ -0,0 +1,131 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos_ros/active_window/tf_icp_publisher.h" + +#include +#include +#include +#include +#include +#include + +namespace khronos { +namespace { + +static const auto registration = + config::RegistrationWithConfig("TfIcpPublisher"); + +} // namespace + +void declare_config(TfIcpPublisher::Config& config) { + using namespace config; + name("TfIcpPublisher"); + field(config.verbosity, "verbosity"); + field(config.pre_icp_odom_frame, "pre_icp_odom_frame"); + field(config.odom_frame, "odom_frame"); +} + +TfIcpPublisher::TfIcpPublisher(const Config& cfg, const ianvs::NodeHandle* nh) + : config(config::checkValid(cfg)), + nh_(nh ? *nh / "tf_icp_publisher" : ianvs::NodeHandle::this_node("tf_icp_publisher")) { + tf_broadcaster_ = std::make_unique(nh_.node()); + tf_buffer_ = std::make_shared(nh_.clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Seed identity so map → odom TF chain is resolvable before the first ICP result. + broadcastTransform(Eigen::Isometry3d::Identity()); + MLOG(1) << "[TfIcpPublisher] Initialized (" << config.pre_icp_odom_frame << " -> " + << getOdomFrame() << "). Seeded identity transform."; +} + +void TfIcpPublisher::call(const DynamicSceneGraph::Ptr& /*dsg*/, + const std::vector& /*removed_objects*/, + const std::vector& /*newly_added_tracks*/, + const Eigen::Isometry3d& current_T_prior) const { + // Look up map → pre_icp_odom (the ROMAN-only result = pre_icp_odom_T_map). + Eigen::Isometry3d pre_icp_odom_T_map; + try { + const auto tf = tf_buffer_->lookupTransform("map", config.pre_icp_odom_frame, tf2::TimePointZero); + const auto& t = tf.transform.translation; + const auto& r = tf.transform.rotation; + pre_icp_odom_T_map = Eigen::Isometry3d::Identity(); + pre_icp_odom_T_map.translation() << t.x, t.y, t.z; + pre_icp_odom_T_map.linear() = Eigen::Quaterniond(r.w, r.x, r.y, r.z).toRotationMatrix(); + } catch (const tf2::TransformException& e) { + MLOG(3) << "[TfIcpPublisher] TF lookup map -> " << config.pre_icp_odom_frame + << " failed: " << e.what() << ". Broadcasting identity."; + broadcastTransform(Eigen::Isometry3d::Identity()); + return; + } + + // ICP delta: odom_T_pre_icp_odom = odom_T_map * map_T_pre_icp_odom + // current_T_prior = odom_T_map (full ROMAN + ICP result from AWCD) + // pre_icp_odom_T_map = ROMAN-only result (no ICP) + const Eigen::Isometry3d delta = current_T_prior * pre_icp_odom_T_map.inverse(); + broadcastTransform(delta); +} + +std::string TfIcpPublisher::getOdomFrame() const { + if (!config.odom_frame.empty()) { + return config.odom_frame; + } + return hydra::GlobalInfo::instance().getFrames().odom; +} + +void TfIcpPublisher::broadcastTransform(const Eigen::Isometry3d& pre_icp_odom_T_odom) const { + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = nh_.now(); + tf_msg.header.frame_id = config.pre_icp_odom_frame; + tf_msg.child_frame_id = getOdomFrame(); + + const auto& t = pre_icp_odom_T_odom.translation(); + tf_msg.transform.translation.x = t.x(); + tf_msg.transform.translation.y = t.y(); + tf_msg.transform.translation.z = t.z(); + const Eigen::Quaterniond q(pre_icp_odom_T_odom.rotation()); + tf_msg.transform.rotation.x = q.x(); + tf_msg.transform.rotation.y = q.y(); + tf_msg.transform.rotation.z = q.z(); + tf_msg.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(tf_msg); +} + +} // namespace khronos diff --git a/khronos_ros/src/active_window/tf_transformation_getter.cpp b/khronos_ros/src/active_window/tf_transformation_getter.cpp new file mode 100644 index 0000000..7b2ba3e --- /dev/null +++ b/khronos_ros/src/active_window/tf_transformation_getter.cpp @@ -0,0 +1,112 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos_ros/active_window/tf_transformation_getter.h" + +#include +#include +#include +#include +#include +#include + +namespace khronos { +namespace { + +static const auto registration = + config::RegistrationWithConfig("TFTransformationGetter"); + +} // namespace + +void declare_config(TFTransformationGetter::Config& config) { + using namespace config; + name("TFTransformationGetter"); + field(config.verbosity, "verbosity"); + field(config.prior_frame_id, "prior_frame_id"); + field(config.robot_frame_id, "robot_frame_id"); + field(config.tf_change_threshold_m, "tf_change_threshold_m"); +} + +TFTransformationGetter::TFTransformationGetter(const Config& cfg) + : config(config::checkValid(cfg)) { + auto nh = ianvs::NodeHandle::this_node(); + tf_buffer_ = std::make_shared(nh.clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + MLOG(1) << "[TFTransformationGetter] TF listener created (" << config.prior_frame_id << " -> " + << getRobotFrame() << ")"; +} + +std::optional TFTransformationGetter::getTransformation() const { + try { + const auto tf = + tf_buffer_->lookupTransform(config.prior_frame_id, getRobotFrame(), tf2::TimePointZero); + + // Convert geometry_msgs::TransformStamped to Eigen: this is map_T_{robot}/odom. + const auto& t = tf.transform.translation; + const auto& r = tf.transform.rotation; + Eigen::Isometry3d map_T_odom = Eigen::Isometry3d::Identity(); + map_T_odom.translation() << t.x, t.y, t.z; + map_T_odom.linear() = Eigen::Quaterniond(r.w, r.x, r.y, r.z).toRotationMatrix(); + + // current_T_prior = inverse of map_T_odom (odom frame is "current"). + const Eigen::Isometry3d current_T_prior = map_T_odom.inverse(); + + const double delta = (current_T_prior.translation() - last_reported_.translation()).norm(); + if (has_last_ && delta <= config.tf_change_threshold_m) { + return std::nullopt; + } + + MLOG(1) << "[TFTransformationGetter] TF changed by " << delta << " m, reporting new transform."; + last_reported_ = current_T_prior; + has_last_ = true; + return current_T_prior; + } catch (const tf2::TransformException& e) { + MLOG(3) << "[TFTransformationGetter] TF lookup failed: " << e.what(); + return std::nullopt; + } +} + +std::string TFTransformationGetter::getRobotFrame() const { + if (!config.robot_frame_id.empty()) { + return config.robot_frame_id; + } + return hydra::GlobalInfo::instance().getFrames().odom; +} + +} // namespace khronos diff --git a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp new file mode 100644 index 0000000..e01bfaf --- /dev/null +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -0,0 +1,225 @@ +/** ----------------------------------------------------------------------------- + * Copyright (c) 2024 Massachusetts Institute of Technology. + * All Rights Reserved. + * + * AUTHORS: Lukas Schmid , Marcus Abate , + * Yun Chang , Luca Carlone + * AFFILIATION: MIT SPARK Lab, Massachusetts Institute of Technology + * YEAR: 2024 + * SOURCE: https://github.com/MIT-SPARK/Khronos + * LICENSE: BSD 3-Clause + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * -------------------------------------------------------------------------- */ + +#include "khronos_ros/visualization/active_window_change_detector_visualizer.h" + +#include +#include +#include +#include + +#include "khronos_ros/visualization/visualization_utils.h" + +namespace khronos { +namespace { + +static const auto registration = + config::RegistrationWithConfig( + "ActiveWindowChangeDetectorVisualizer"); + +} + +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config) { + using namespace config; + name("ActiveWindowChangeDetectorVisualizer"); + field(config.verbosity, "verbosity"); + field(config.global_frame_name, "global_frame_name"); + field(config.renderer, "renderer"); + field(config.mesh, "mesh"); + field(config.queue_size, "queue_size"); + field(config.bounding_box_line_width, "bounding_box_line_width"); + field(config.min_draw_period_s, "min_draw_period_s"); + // TODO(multy): add checks for the fields. +} + +ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( + const Config& config, + const ianvs::NodeHandle* nh) + : config(config::checkValid(config)), + nh_(nh ? *nh / "change_detector_visualizer" + : ianvs::NodeHandle::this_node("change_detector_visualizer")) { + // Initialize renderer and mesh plugin + renderer_ = std::make_shared(config.renderer, nh_); + mesh_plugin_ = std::make_shared(config.mesh, nh_, "prior_mesh"); + has_drawn_ = false; + last_draw_time_ = std::nullopt; + + // Initialize publishers + object_bbox_pub_ = + nh_.create_publisher("changed_object_bounding_boxes", config.queue_size); + MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Initialized."; +} + +void ActiveWindowChangeDetectorVisualizer::call( + const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_objects, + const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const { + // Throttle visualization redraws to reduce flicker from high-frequency calls. + if (config.min_draw_period_s > 0.0) { + const rclcpp::Time now = nh_.now(); + if (last_draw_time_.has_value() && (now - *last_draw_time_).seconds() < config.min_draw_period_s) { + return; + } + last_draw_time_ = now; + } + + drawPriorGraph(dsg); + + // print out removed object ids + MLOG(3) << "[ActiveWindowChangeDetectorVisualizer] Object "; + for (const auto& obj : removed_objects) { + MLOG(3) << spark_dsg::NodeSymbol(obj.id).str() << ", "; + } + MLOG(3) << " are removed"; + // set stamps for all visualizations + stamp_ = rclcpp::Time(0); + stamp_is_set_ = true; + + // Visualize removed objects + visualizeChangedObjects(dsg, removed_objects); + visualizeAddedObjects(newly_added_tracks, current_T_prior); + stamp_is_set_ = false; +} + +void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const { + if (!dsg) { + MLOG(2) << "[ChangeDetectorVisualizer] No prior graph provided, skipping draw"; + return; + } + + // Only redraw if renderer has changes or we haven't drawn yet. + if (has_drawn_ && !renderer_->hasChange()) { + MLOG(3) << "[ChangeDetectorVisualizer] Prior graph already drawn and no changes detected, " + "skipping draw"; + return; + } + + MLOG(2) << "[ChangeDetectorVisualizer] Drawing prior graph"; + + std_msgs::msg::Header header; + header.frame_id = config.global_frame_name; + header.stamp = rclcpp::Time(0); + + renderer_->draw(header, *dsg); + mesh_plugin_->draw(header, *dsg); + renderer_->clearChangeFlag(); + has_drawn_ = true; +} + +void ActiveWindowChangeDetectorVisualizer::visualizeChangedObjects( + const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_objects) const { + if (object_bbox_pub_->get_subscription_count() == 0u) { + return; + } + + // draw red bounding boxes for removed objects + MarkerArray new_markers; + new_markers.markers.reserve(removed_objects.size()); + + std_msgs::msg::Header header; + header.frame_id = config.global_frame_name; + header.stamp = getStamp(); + for (const auto& obj : removed_objects) { + const auto& object_node = dsg->getNode(obj.id); + const auto* khronos_attrs = object_node.tryAttributes(); + if (!khronos_attrs || !khronos_attrs->bounding_box.isValid()) { + continue; + } + auto& marker = new_markers.markers.emplace_back(setBoundingBox( + khronos_attrs->bounding_box, Color(255, 0, 0, 255), header, config.bounding_box_line_width)); + // Use stable identity-based id (lower 32 bits of NodeId) so the same object + // always gets the same marker id across frames. Namespace "removed_objects" + // avoids collision with green added-object markers on the same publisher. + marker.ns = "removed_objects"; + marker.id = static_cast(obj.id & 0xffffffff); + } + + MarkerArray msg; + object_bbox_tracker_.add(new_markers, msg); + object_bbox_tracker_.clearPrevious(header, msg); + if (!msg.markers.empty()) { + object_bbox_pub_->publish(msg); + } +} + +void ActiveWindowChangeDetectorVisualizer::visualizeAddedObjects( + const std::vector& newly_added_tracks, + const Eigen::Isometry3d& current_T_prior) const { + if (object_bbox_pub_->get_subscription_count() == 0u) { + return; + } + + // Transform track bounding boxes (in current/odom frame) into map frame for RViz. + const Eigen::Isometry3d prior_T_current = current_T_prior.inverse(); + + MarkerArray new_markers; + new_markers.markers.reserve(newly_added_tracks.size()); + + std_msgs::msg::Header header; + header.frame_id = config.global_frame_name; + header.stamp = getStamp(); + for (const Track& track : newly_added_tracks) { + MLOG(2) << "[ActiveWindowChangeDetectorVisualizer] Visualizing newly added track with id " << track.id; + BoundingBox bbox = track.last_bounding_box; + if (!bbox.isValid()) { + continue; + } + bbox.transform(prior_T_current); + auto& marker = new_markers.markers.emplace_back( + setBoundingBox(bbox, Color(0, 255, 0, 255), header, config.bounding_box_line_width)); + // Use stable track.id as marker id (monotonically increasing, never reused by MaxIoUTracker). + // Namespace "added_objects" avoids collision with red removed-object markers on the same publisher. + marker.ns = "added_objects"; + marker.id = static_cast(track.id); + } + + MarkerArray msg; + added_object_bbox_tracker_.add(new_markers, msg); + added_object_bbox_tracker_.clearPrevious(header, msg); + if (!msg.markers.empty()) { + object_bbox_pub_->publish(msg); + } +} + +} // namespace khronos diff --git a/khronos_ros/src/visualization/active_window_visualizer.cpp b/khronos_ros/src/visualization/active_window_visualizer.cpp index 9279b6a..e06ae31 100644 --- a/khronos_ros/src/visualization/active_window_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_visualizer.cpp @@ -75,9 +75,15 @@ void declare_config(ActiveWindowVisualizer::Config& config) { field(config.id_color_revolutions, "id_color_revolutions"); field(config.bounding_box_line_width, "bounding_box_line_width"); field(config.sensor_displays, "sensor_displays"); + field(config.marker_opacity, "marker_opacity"); + field(config.num_slices, "num_slices"); + field(config.slice_distance_multiplier, "slice_distance_multiplier"); check(config.queue_size, GT, 0, "queue_size"); + check(config.num_slices, GT, 0, "num_slices"); check(config.dynamic_point_scale, GT, 0, "dynamic_point_scale"); + check(config.marker_opacity, GE, 0, "marker_opacity"); + check(config.marker_opacity, LE, 255, "marker_opacity"); checkCondition(!config.global_frame_name.empty(), "param 'global_frame_name' must not be empty"); check(config.id_color_revolutions, GT, 0, "id_color_revolutions"); check(config.bounding_box_line_width, GT, 0, "bounding_box_line_width"); @@ -364,35 +370,50 @@ void ActiveWindowVisualizer::visualizeEverFreeSlice(const VolumetricMap& map, if (config.slice_height_is_relative) { slice_height += robot_height; } - const Point slice_coords(0, 0, slice_height); - const VoxelKey slice_key = map.getTrackingLayer()->getVoxelKey(slice_coords); + // const Point slice_coords(0, 0, slice_height); + // const VoxelKey slice_key = map.getTrackingLayer()->getVoxelKey(slice_coords); + + std::vector slice_keys; + for (int i = 0; i < config.num_slices; ++i) { + int num_distance = (i + 1) / 2; + float current_slice_height = slice_height + std::pow(-1, i) * num_distance * + config.slice_distance_multiplier * + map.config.voxel_size; + const Point current_slice_coords(0, 0, current_slice_height); + slice_keys.push_back(layer.getVoxelKey(current_slice_coords)); + } // Visualize. - for (const TrackingBlock& block : layer) { - if (block.index.z() != slice_key.first.z()) { - continue; - } + for (const VoxelKey& slice_key : slice_keys) { + for (const TrackingBlock& block : layer) { + if (block.index.z() != slice_key.first.z()) { + continue; + } - for (size_t x = 0; x < block.voxels_per_side; ++x) { - for (size_t y = 0; y < block.voxels_per_side; ++y) { - const VoxelIndex voxel_index(x, y, slice_key.second.z()); - const TrackingVoxel& voxel = block.getVoxel(voxel_index); + for (size_t x = 0; x < block.voxels_per_side; ++x) { + for (size_t y = 0; y < block.voxels_per_side; ++y) { + const VoxelIndex voxel_index(x, y, slice_key.second.z()); + const TrackingVoxel& voxel = block.getVoxel(voxel_index); - const bool is_unknown = voxel.last_observed == 0u; - if (is_unknown && !config.show_unknown_voxels) { - continue; - } + const bool is_unknown = voxel.last_observed == 0u; + if (is_unknown && !config.show_unknown_voxels) { + continue; + } - Point coords = block.getVoxelPosition(voxel_index); - msg.points.emplace_back(setPoint(coords)); - if (is_unknown) { - msg.colors.emplace_back(setColor(Color::gray())); - } else if (voxel.ever_free) { - // Free voxel. - msg.colors.emplace_back(setColor(Color::green())); - } else { - // Occupied voxel. - msg.colors.emplace_back(setColor(Color::red())); + Point coords = block.getVoxelPosition(voxel_index); + msg.points.emplace_back(setPoint(coords)); + if (is_unknown) { + msg.colors.emplace_back( + setColor(Color(125, 125, 125, config.marker_opacity))); // Gray for unknown. + } else if (voxel.ever_free) { + // Free voxel. + msg.colors.emplace_back( + setColor(Color(0, 255, 0, config.marker_opacity))); // Green for ever free. + } else { + // Occupied voxel. + msg.colors.emplace_back( + setColor(Color(255, 0, 0, config.marker_opacity))); // Red for occupied. + } } } }