From 80d38b522d7abe1e219e6a285a24ba631e684849 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 21 Jan 2026 10:26:45 -0500 Subject: [PATCH 01/27] Initial code structure for change detection using khronos sink --- khronos/CMakeLists.txt | 1 + .../active_window_change_detector.h | 82 +++++++++++++++++++ khronos/src/active_window/active_window.cpp | 12 +++ .../active_window_change_detector.cpp | 73 +++++++++++++++++ 4 files changed, 168 insertions(+) create mode 100644 khronos/include/khronos/active_window/change_detection/active_window_change_detector.h create mode 100644 khronos/src/active_window/change_detection/active_window_change_detector.cpp diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index 8c869eb..3f59e25 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -28,6 +28,7 @@ include(GNUInstallDirs) add_library(${PROJECT_NAME} src/active_window/active_window.cpp + src/active_window/change_detection/active_window_change_detector.cpp src/active_window/data/frame_data_buffer.cpp src/active_window/data/track.cpp src/active_window/integration/object_integrator.cpp 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..6b82159 --- /dev/null +++ b/khronos/include/khronos/active_window/change_detection/active_window_change_detector.h @@ -0,0 +1,82 @@ +/** ----------------------------------------------------------------------------- + * 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/active_window.h" + +namespace khronos { + +class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { + public: + // Config. + struct Config { + //! Verbosity level. + int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; + + //! Path to prior map to use for change detection. + std::filesystem::path prior_map_path; + } const config; + + // Construction. + explicit ActiveWindowChangeDetector(const Config& config); + virtual ~ActiveWindowChangeDetector() = default; + + /** + * @brief TODO + * @param map The current map to visualize. + * @param data The current data after processing to visualize. + * @param tracks The current tracks in the active window to visualize. If a bounding box for a + * track is newly computed it will be stored in the track. + */ + void call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const override; + + void loadPriorMap(/* params */); + + private: + // prior map + + // relative pose to the prior map (pose lookup, prior map frame relative to the current map frame) + +}; + +void declare_config(ActiveWindowChangeDetector::Config& config); + +} // namespace khronos 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..4e72cdc --- /dev/null +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -0,0 +1,73 @@ +/** ----------------------------------------------------------------------------- + * 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 +#include "khronos/active_window/change_detection/active_window_change_detector.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.prior_map_path, "prior_map_path"); + 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)){ + +} + +void ActiveWindowChangeDetector::call(const FrameData& data, + const VolumetricMap& map, + const Tracks& tracks) const { +} + +void ActiveWindowChangeDetector::loadPriorMap(/* params */) { + +} + +} // namespace khronos From bd150bd2e2ca0b08eac1a084126b317aaf598825 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Sat, 24 Jan 2026 14:21:18 -0500 Subject: [PATCH 02/27] First basic draft of AWCD for remobed object only. - loading prior map - function to set prior->current map transform - function to filter prior object node in current voxel map (by centroid) - Call function to check object vertices in voxel map (if high confidence free space) --- .../active_window_change_detector.h | 44 ++++- .../active_window_change_detector.cpp | 153 ++++++++++++++++-- 2 files changed, 183 insertions(+), 14 deletions(-) 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 index 6b82159..9950b15 100644 --- 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 @@ -37,8 +37,8 @@ #pragma once - #include +#include #include "khronos/active_window/active_window.h" @@ -49,10 +49,12 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { // Config. struct Config { //! Verbosity level. - int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; + int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; //! Path to prior map to use for change detection. std::filesystem::path prior_map_path; + + float removal_vertex_free_ratio_threshold = 0.8f; } const config; // Construction. @@ -70,11 +72,43 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { void loadPriorMap(/* params */); + bool isPriorPointFree(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); + + /** + * @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; + private: - // prior map + // Prior map as a 3D scene graph. + DynamicSceneGraph::Ptr prior_graph_; - // relative pose to the prior map (pose lookup, prior map frame relative to the current map frame) - + // Transform from prior map frame to current map frame. + // current_point = current_T_prior_ * prior_point + Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); }; void declare_config(ActiveWindowChangeDetector::Config& config); 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 index 4e72cdc..620f1b8 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -35,39 +35,174 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * -------------------------------------------------------------------------- */ -#include #include "khronos/active_window/change_detection/active_window_change_detector.h" +#include +#include +#include + namespace khronos { namespace { -static const auto registration = - config::RegistrationWithConfig("ActiveWindowChangeDetector"); +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.prior_map_path, "prior_map_path"); check(config.prior_map_path, "prior_map_path"); -// check(config.prior_map_path, "prior_map_path", ".spark_dsg"); // Add the check back sometimes. + // check(config.prior_map_path, "prior_map_path", ".spark_dsg"); // Add the + // check back sometimes. } ActiveWindowChangeDetector::ActiveWindowChangeDetector(const Config& config) - : config(config::checkValid(config)){ + : config(config::checkValid(config)) { + LOG(INFO) << "[Khronos Active Window Change Detector] Initialized with prior map path: " + << config.prior_map_path; + CLOG(1) << "[Khronos Active Window Change Detector] Verbosity level: " << config.verbosity; + loadPriorMap(); + LOG(INFO) << "[Khronos Active Window Change Detector] Loaded prior scene graph with " + << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); + // test(multy) + spark_dsg::NodeSymbol object_symbol('O', 0); + const auto& object_node = prior_graph_->getNode(object_symbol); + const auto& object_attrs = object_node.attributes(); + // print out the attributes info + LOG(INFO) << "[Khronos Active Window Change Detector] Example object node attributes: " + << object_attrs; } void ActiveWindowChangeDetector::call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const { + // 1. Find all object nodes in the prior graph within current volumetric map bounds + const auto objects_id_in_bounds = findPriorObjectsInMapBounds(map); + + std::vector removed_objects; + + // 2. For each object node, get mesh vertices and transform to current frame + for (const auto object_id : objects_id_in_bounds) { + const auto& object_node = prior_graph_->getNode(object_id); + + // Cast to KhronosObjectAttributes to access mesh + const auto* khronos_attrs = object_node.tryAttributes(); + if (!khronos_attrs) { + CLOG(2) << "[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) { + continue; + } + + int num_vertices_in_free_space = 0; + + // 3. For each vertex, check if it's in free space + 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 (isPriorPointFree(vertex_current, map)) { + ++num_vertices_in_free_space; + } + } + + // 4. If more than threshold% of vertices are in free space, mark as removed + const float free_ratio = static_cast(num_vertices_in_free_space) / + static_cast(mesh.numVertices()); + + if (free_ratio >= config.removal_vertex_free_ratio_threshold) { + removed_objects.push_back(object_id); + CLOG(1) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " detected as REMOVED (free ratio: " << free_ratio << ")"; + } else { + CLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + << " still present (free ratio: " << free_ratio << ")"; + } + } + + LOG(INFO) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() + << " removed objects out of " << objects_id_in_bounds.size() << " checked"; +} + +void ActiveWindowChangeDetector::loadPriorMap(/* params */) { + prior_graph_ = + DynamicSceneGraph::load(config.prior_map_path / "hydra" / "backend" / "dsg_with_mesh.json"); +} + +bool ActiveWindowChangeDetector::isPriorPointFree(const Point& point_in_map, + const VolumetricMap& map) const { + auto* voxel = map.getTrackingLayer()->getVoxelPtr(point_in_map); + if (voxel && voxel->ever_free) { + return true; + } + return false; +} + +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.getBlockPtr(tsdf_layer.getBlockIndex(point.cast())) != nullptr; +} + +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); + CLOG(3) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(node_id).str() + << " is within map bounds at position (current frame): " + << position_in_current.transpose(); + } + } + + CLOG(1) << "[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) { + current_T_prior_ = current_T_prior; + LOG(INFO) << "[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(); } -void ActiveWindowChangeDetector::loadPriorMap(/* params */) { - +Point ActiveWindowChangeDetector::transformPriorToCurrentFrame( + const Eigen::Vector3d& point_in_prior) const { + // 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(); } } // namespace khronos From 8974261bf2f34cd76788d4faf897e01627e99e41 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 26 Jan 2026 15:09:35 -0500 Subject: [PATCH 03/27] Minor edition of code styles. --- .../active_window_change_detector.h | 18 +++--- .../active_window_change_detector.cpp | 63 +++++++++---------- 2 files changed, 40 insertions(+), 41 deletions(-) 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 index 9950b15..b9d5be0 100644 --- 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 @@ -54,6 +54,7 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! 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 to consider it removed. float removal_vertex_free_ratio_threshold = 0.8f; } const config; @@ -62,15 +63,14 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { virtual ~ActiveWindowChangeDetector() = default; /** - * @brief TODO - * @param map The current map to visualize. - * @param data The current data after processing to visualize. - * @param tracks The current tracks in the active window to visualize. If a bounding box for a - * track is newly computed it will be stored in the track. + * @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(/* params */); + void loadPriorMap(); bool isPriorPointFree(const Point& point_in_map, const VolumetricMap& map) const; @@ -103,11 +103,11 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { Point transformPriorToCurrentFrame(const Eigen::Vector3d& point_in_prior) const; private: - // Prior map as a 3D scene graph. + //! 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 + //! Transform from prior map frame to current map frame. + //! current_point = current_T_prior_ * prior_point Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); }; 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 index 620f1b8..33fcdb9 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -63,20 +63,11 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { ActiveWindowChangeDetector::ActiveWindowChangeDetector(const Config& config) : config(config::checkValid(config)) { - LOG(INFO) << "[Khronos Active Window Change Detector] Initialized with prior map path: " - << config.prior_map_path; - CLOG(1) << "[Khronos Active Window Change Detector] Verbosity level: " << config.verbosity; + CLOG(1) << "[Khronos Active Window Change Detector] Initialized with prior map path: " + << config.prior_map_path; loadPriorMap(); - LOG(INFO) << "[Khronos Active Window Change Detector] Loaded prior scene graph with " - << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); - - // test(multy) - spark_dsg::NodeSymbol object_symbol('O', 0); - const auto& object_node = prior_graph_->getNode(object_symbol); - const auto& object_attrs = object_node.attributes(); - // print out the attributes info - LOG(INFO) << "[Khronos Active Window Change Detector] Example object node attributes: " - << object_attrs; + CLOG(1) << "[Khronos Active Window Change Detector] Loaded prior scene graph with " + << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); } void ActiveWindowChangeDetector::call(const FrameData& data, @@ -120,13 +111,13 @@ void ActiveWindowChangeDetector::call(const FrameData& data, } } - // 4. If more than threshold% of vertices are in free space, mark as removed - const float free_ratio = static_cast(num_vertices_in_free_space) / - static_cast(mesh.numVertices()); + // 4. If more than threshold % of vertices are in free space, mark as removed + const float free_ratio = + static_cast(num_vertices_in_free_space) / static_cast(mesh.numVertices()); if (free_ratio >= config.removal_vertex_free_ratio_threshold) { removed_objects.push_back(object_id); - CLOG(1) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + CLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " detected as REMOVED (free ratio: " << free_ratio << ")"; } else { CLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() @@ -134,29 +125,35 @@ void ActiveWindowChangeDetector::call(const FrameData& data, } } - LOG(INFO) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() - << " removed objects out of " << objects_id_in_bounds.size() << " checked"; + // 5. TODO (multy): need ways to report the problem or even visualize it. + CLOG(1) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() + << " removed objects out of " << objects_id_in_bounds.size() << " checked"; + // print out removed object ids + CLOG(1) << "[ActiveWindowChangeDetector] Object "; + for (const auto& id : removed_objects) { + CLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; + } + CLOG(1) << " are removed"; } -void ActiveWindowChangeDetector::loadPriorMap(/* params */) { - prior_graph_ = - DynamicSceneGraph::load(config.prior_map_path / "hydra" / "backend" / "dsg_with_mesh.json"); +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); } bool ActiveWindowChangeDetector::isPriorPointFree(const Point& point_in_map, const VolumetricMap& map) const { - auto* voxel = map.getTrackingLayer()->getVoxelPtr(point_in_map); - if (voxel && voxel->ever_free) { - return true; - } - return false; + 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.getBlockPtr(tsdf_layer.getBlockIndex(point.cast())) != nullptr; + return tsdf_layer.hasBlock(point.cast()); } std::vector ActiveWindowChangeDetector::findPriorObjectsInMapBounds( @@ -192,14 +189,16 @@ std::vector ActiveWindowChangeDetector::findPriorObjectsInMap void ActiveWindowChangeDetector::setCurrentToPriorTransform( const Eigen::Isometry3d& current_T_prior) { current_T_prior_ = current_T_prior; - LOG(INFO) << "[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(); + CLOG(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(); From c64e7e1e86a0547e0565617ab911a1f38a974c14 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 26 Jan 2026 15:32:20 -0500 Subject: [PATCH 04/27] Modify to use MLOG (but an earlier version) --- .../active_window_change_detector.h | 10 ++++++-- .../active_window_change_detector.cpp | 24 +++++++++---------- 2 files changed, 20 insertions(+), 14 deletions(-) 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 index b9d5be0..3fb0c60 100644 --- 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 @@ -42,14 +42,20 @@ #include "khronos/active_window/active_window.h" +#include "hydra/utils/logging.h" + namespace khronos { class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { public: // Config. - struct Config { + struct Config : hydra::VerbosityConfig { //! Verbosity level. - int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; + // int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; + // TODO(multy): after hydra is updated, should change it to also include a prefix like: "[Active + // Window Change Detector] " + Config() + : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} //! Path to prior map to use for change detection. std::filesystem::path prior_map_path; 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 index 33fcdb9..5fbe964 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -63,10 +63,10 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { ActiveWindowChangeDetector::ActiveWindowChangeDetector(const Config& config) : config(config::checkValid(config)) { - CLOG(1) << "[Khronos Active Window Change Detector] Initialized with prior map path: " + MLOG(1) << "[Khronos Active Window Change Detector] Initialized with prior map path: " << config.prior_map_path; loadPriorMap(); - CLOG(1) << "[Khronos Active Window Change Detector] Loaded prior scene graph with " + MLOG(1) << "[Khronos Active Window Change Detector] Loaded prior scene graph with " << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); } @@ -85,7 +85,7 @@ void ActiveWindowChangeDetector::call(const FrameData& data, // Cast to KhronosObjectAttributes to access mesh const auto* khronos_attrs = object_node.tryAttributes(); if (!khronos_attrs) { - CLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " does not have KhronosObjectAttributes, skipping"; continue; } @@ -117,23 +117,23 @@ void ActiveWindowChangeDetector::call(const FrameData& data, if (free_ratio >= config.removal_vertex_free_ratio_threshold) { removed_objects.push_back(object_id); - CLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " detected as REMOVED (free ratio: " << free_ratio << ")"; } else { - CLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " still present (free ratio: " << free_ratio << ")"; } } // 5. TODO (multy): need ways to report the problem or even visualize it. - CLOG(1) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() + MLOG(1) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() << " removed objects out of " << objects_id_in_bounds.size() << " checked"; // print out removed object ids - CLOG(1) << "[ActiveWindowChangeDetector] Object "; + MLOG(1) << "[ActiveWindowChangeDetector] Object "; for (const auto& id : removed_objects) { - CLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; + MLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; } - CLOG(1) << " are removed"; + MLOG(1) << " are removed"; } void ActiveWindowChangeDetector::loadPriorMap() { @@ -174,13 +174,13 @@ std::vector ActiveWindowChangeDetector::findPriorObjectsInMap if (isPointInMapBounds(position_in_current, map)) { objects_in_bounds.push_back(node_id); - CLOG(3) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(node_id).str() + MLOG(3) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(node_id).str() << " is within map bounds at position (current frame): " << position_in_current.transpose(); } } - CLOG(1) << "[ActiveWindowChangeDetector] Found " << objects_in_bounds.size() + MLOG(1) << "[ActiveWindowChangeDetector] Found " << objects_in_bounds.size() << " prior objects within current map bounds"; return objects_in_bounds; @@ -189,7 +189,7 @@ std::vector ActiveWindowChangeDetector::findPriorObjectsInMap void ActiveWindowChangeDetector::setCurrentToPriorTransform( const Eigen::Isometry3d& current_T_prior) { current_T_prior_ = current_T_prior; - CLOG(1) << "[ActiveWindowChangeDetector] Updated current_T_prior transform:\n" + 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(); From d0af06bbb03ac59d4db94749441ccb63609b987c Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 2 Feb 2026 13:17:57 -0500 Subject: [PATCH 05/27] Added activew window change detection visualizer as active window sink. Later, it should be move to be a sink for the change detector sink --- ...active_window_change_detector_visualizer.h | 94 +++++++++++++ ...tive_window_change_detector_visualizer.cpp | 128 ++++++++++++++++++ 2 files changed, 222 insertions(+) create mode 100644 khronos_ros/include/khronos_ros/visualization/active_window_change_detector_visualizer.h create mode 100644 khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp 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..b477fa7 --- /dev/null +++ b/khronos_ros/include/khronos_ros/visualization/active_window_change_detector_visualizer.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 +#include +#include +#include + +namespace khronos { + +class ActiveWindowChangeDetectorVisualizer : public ActiveWindow::KhronosSink { + public: + struct Config { + int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; + + // Frame in which to publish visualizations. + std::string global_frame_name = hydra::GlobalInfo::instance().getFrames().map; + + // Path to prior map DSG file. + std::filesystem::path prior_map_path; + + // Scene graph renderer config. + hydra::SceneGraphRenderer::Config renderer; + + // Mesh plugin config (optional - if coloring is not set, uses mesh colors). + hydra::MeshPlugin::Config mesh; + } const config; + + explicit ActiveWindowChangeDetectorVisualizer(const Config& config, + const ianvs::NodeHandle* nh = nullptr); + virtual ~ActiveWindowChangeDetectorVisualizer() = default; + + // KhronosSink callback - called each frame. + void call(const FrameData& data, + const VolumetricMap& map, + const Tracks& tracks) const override; + + // Load/reload the prior graph from file. + void loadPriorGraph(); + + private: + void drawPriorGraph() const; + + ianvs::NodeHandle nh_; + spark_dsg::DynamicSceneGraph::Ptr prior_graph_; + std::shared_ptr renderer_; + std::shared_ptr mesh_plugin_; + mutable bool has_drawn_ = false; +}; + +void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config); + +} // 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..2b336d9 --- /dev/null +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -0,0 +1,128 @@ +/** ----------------------------------------------------------------------------- + * 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 + +namespace khronos { +namespace { + +static const auto registration = + config::RegistrationWithConfig( + "ActiveWindowChangeDetectorVisualizer"); + +} + +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.prior_map_path, "prior_map_path"); + field(config.renderer, "renderer"); + field(config.mesh, "mesh"); +} + +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")) { + renderer_ = std::make_shared(config.renderer, nh_); + mesh_plugin_ = std::make_shared(config.mesh, nh_, "prior_mesh"); + loadPriorGraph(); +} + +void ActiveWindowChangeDetectorVisualizer::loadPriorGraph() { + if (config.prior_map_path.empty()) { + CLOG(1) << "[ChangeDetectorVisualizer] No prior map path specified, skipping load"; + return; + } + + if (!std::filesystem::exists(config.prior_map_path)) { + LOG(WARNING) << "[ChangeDetectorVisualizer] Prior map not found at: " << config.prior_map_path; + return; + } + + CLOG(1) << "[ChangeDetectorVisualizer] Loading prior map from: " << config.prior_map_path; + prior_graph_ = spark_dsg::DynamicSceneGraph::load(config.prior_map_path); + has_drawn_ = false; + + if (prior_graph_) { + CLOG(1) << "[ChangeDetectorVisualizer] Prior map loaded with " << prior_graph_->numNodes() + << " nodes"; + } +} + +void ActiveWindowChangeDetectorVisualizer::call(const FrameData& /* data */, + const VolumetricMap& /* map */, + const Tracks& /* tracks */) const { + drawPriorGraph(); +} + +void ActiveWindowChangeDetectorVisualizer::drawPriorGraph() const { + if (!prior_graph_) { + return; + } + + // Only redraw if renderer has changes or we haven't drawn yet. + if (has_drawn_ && !renderer_->hasChange()) { + CLOG(3) << "[ChangeDetectorVisualizer] Prior graph already drawn and no changes detected, " + "skipping draw"; + return; + } + + CLOG(2) << "[ChangeDetectorVisualizer] Drawing prior graph"; + + std_msgs::msg::Header header; + header.frame_id = config.global_frame_name; + header.stamp = nh_.now(); + + renderer_->draw(header, *prior_graph_); + mesh_plugin_->draw(header, *prior_graph_); + renderer_->clearChangeFlag(); + has_drawn_ = true; +} + +} // namespace khronos From 7b1c5c9c8c620e532678b8e277bf579d688f05df Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 3 Feb 2026 21:20:13 -0500 Subject: [PATCH 06/27] add sinks for active window change detector, now visualize the prior map through the sink. --- .../active_window_change_detector.h | 22 +++++++- .../active_window_change_detector.cpp | 25 +++++---- khronos_ros/CMakeLists.txt | 1 + ...active_window_change_detector_visualizer.h | 36 +++++++------ ...tive_window_change_detector_visualizer.cpp | 51 +++++++------------ 5 files changed, 72 insertions(+), 63 deletions(-) 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 index 3fb0c60..d3b3cdb 100644 --- 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 @@ -40,14 +40,18 @@ #include #include -#include "khronos/active_window/active_window.h" +#include +#include -#include "hydra/utils/logging.h" +#include "khronos/active_window/active_window.h" namespace khronos { class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { public: + using ActiveWindowCDSink = + hydra::OutputSink&>; + // Config. struct Config : hydra::VerbosityConfig { //! Verbosity level. @@ -62,12 +66,23 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! Ratio of free prior map points of an object's vertices to consider it removed. float removal_vertex_free_ratio_threshold = 0.8f; + + //! Sinks for the change detector output. + std::vector awcd_sinks; } 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. @@ -115,6 +130,9 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! Transform from prior map frame to current map frame. //! current_point = current_T_prior_ * prior_point Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); + + //! Sinks for the change detector output. + ActiveWindowCDSink::List sinks_; }; void declare_config(ActiveWindowChangeDetector::Config& config); 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 index 5fbe964..8669013 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -56,13 +56,15 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { field(config.verbosity, "verbosity"); field(config.removal_vertex_free_ratio_threshold, "removal_vertex_free_ratio_threshold"); field(config.prior_map_path, "prior_map_path"); + field(config.awcd_sinks, "awcd_sinks"); 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)) { + : config(config::checkValid(config)), + sinks_(ActiveWindowCDSink::instantiate(config.awcd_sinks)) { MLOG(1) << "[Khronos Active Window Change Detector] Initialized with prior map path: " << config.prior_map_path; loadPriorMap(); @@ -70,13 +72,19 @@ ActiveWindowChangeDetector::ActiveWindowChangeDetector(const Config& config) << (prior_graph_ ? std::to_string(prior_graph_->numNodes()) + " nodes." : "0 nodes."); } +void ActiveWindowChangeDetector::addKhronosSink(const ActiveWindowCDSink::Ptr& sink) { + if (sink) { + sinks_.push_back(sink); + } +} + void ActiveWindowChangeDetector::call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const { // 1. Find all object nodes in the prior graph within current volumetric map bounds const auto objects_id_in_bounds = findPriorObjectsInMapBounds(map); - std::vector removed_objects; + std::vector removed_object_ids; // 2. For each object node, get mesh vertices and transform to current frame for (const auto object_id : objects_id_in_bounds) { @@ -116,7 +124,7 @@ void ActiveWindowChangeDetector::call(const FrameData& data, static_cast(num_vertices_in_free_space) / static_cast(mesh.numVertices()); if (free_ratio >= config.removal_vertex_free_ratio_threshold) { - removed_objects.push_back(object_id); + removed_object_ids.push_back(object_id); MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " detected as REMOVED (free ratio: " << free_ratio << ")"; } else { @@ -126,14 +134,11 @@ void ActiveWindowChangeDetector::call(const FrameData& data, } // 5. TODO (multy): need ways to report the problem or even visualize it. - MLOG(1) << "[ActiveWindowChangeDetector] Detected " << removed_objects.size() + MLOG(1) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() << " removed objects out of " << objects_id_in_bounds.size() << " checked"; - // print out removed object ids - MLOG(1) << "[ActiveWindowChangeDetector] Object "; - for (const auto& id : removed_objects) { - MLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; - } - MLOG(1) << " are removed"; + + // 6. Call all sinks with the removed objects + ActiveWindowCDSink::callAll(sinks_, prior_graph_, removed_object_ids); } void ActiveWindowChangeDetector::loadPriorMap() { diff --git a/khronos_ros/CMakeLists.txt b/khronos_ros/CMakeLists.txt index d1d6329..3515087 100644 --- a/khronos_ros/CMakeLists.txt +++ b/khronos_ros/CMakeLists.txt @@ -30,6 +30,7 @@ add_library( src/experiments/experiment_directory.cpp src/khronos_pipeline.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( 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 index b477fa7..ff3bf77 100644 --- 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 @@ -41,29 +41,32 @@ #include #include +#include #include #include #include -#include #include +#include "khronos/active_window/change_detection/active_window_change_detector.h" + namespace khronos { -class ActiveWindowChangeDetectorVisualizer : public ActiveWindow::KhronosSink { +class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector::ActiveWindowCDSink { public: - struct Config { - int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; - - // Frame in which to publish visualizations. + 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; - // Path to prior map DSG file. - std::filesystem::path prior_map_path; - - // Scene graph renderer config. + //! Scene graph renderer config. hydra::SceneGraphRenderer::Config renderer; - // Mesh plugin config (optional - if coloring is not set, uses mesh colors). + //! Mesh plugin config (optional - if coloring is not set, uses mesh colors). hydra::MeshPlugin::Config mesh; } const config; @@ -72,18 +75,13 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindow::KhronosSink { virtual ~ActiveWindowChangeDetectorVisualizer() = default; // KhronosSink callback - called each frame. - void call(const FrameData& data, - const VolumetricMap& map, - const Tracks& tracks) const override; - - // Load/reload the prior graph from file. - void loadPriorGraph(); + void call(const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_object_ids) const override; private: - void drawPriorGraph() const; + void drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const; ianvs::NodeHandle nh_; - spark_dsg::DynamicSceneGraph::Ptr prior_graph_; std::shared_ptr renderer_; std::shared_ptr mesh_plugin_; mutable bool has_drawn_ = false; diff --git a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index 2b336d9..01673e4 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -46,7 +46,7 @@ namespace khronos { namespace { static const auto registration = - config::RegistrationWithConfig( "ActiveWindowChangeDetectorVisualizer"); @@ -58,7 +58,6 @@ void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config) { name("ActiveWindowChangeDetectorVisualizer"); field(config.verbosity, "verbosity"); field(config.global_frame_name, "global_frame_name"); - field(config.prior_map_path, "prior_map_path"); field(config.renderer, "renderer"); field(config.mesh, "mesh"); } @@ -71,56 +70,44 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( : ianvs::NodeHandle::this_node("change_detector_visualizer")) { renderer_ = std::make_shared(config.renderer, nh_); mesh_plugin_ = std::make_shared(config.mesh, nh_, "prior_mesh"); - loadPriorGraph(); -} - -void ActiveWindowChangeDetectorVisualizer::loadPriorGraph() { - if (config.prior_map_path.empty()) { - CLOG(1) << "[ChangeDetectorVisualizer] No prior map path specified, skipping load"; - return; - } - - if (!std::filesystem::exists(config.prior_map_path)) { - LOG(WARNING) << "[ChangeDetectorVisualizer] Prior map not found at: " << config.prior_map_path; - return; - } - - CLOG(1) << "[ChangeDetectorVisualizer] Loading prior map from: " << config.prior_map_path; - prior_graph_ = spark_dsg::DynamicSceneGraph::load(config.prior_map_path); has_drawn_ = false; - if (prior_graph_) { - CLOG(1) << "[ChangeDetectorVisualizer] Prior map loaded with " << prior_graph_->numNodes() - << " nodes"; - } + MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Initialized."; } -void ActiveWindowChangeDetectorVisualizer::call(const FrameData& /* data */, - const VolumetricMap& /* map */, - const Tracks& /* tracks */) const { - drawPriorGraph(); +void ActiveWindowChangeDetectorVisualizer::call( + const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_object_ids) const { + drawPriorGraph(dsg); + + // print out removed object ids + MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Object "; + for (const auto& id : removed_object_ids) { + MLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; + } + MLOG(1) << " are removed"; } -void ActiveWindowChangeDetectorVisualizer::drawPriorGraph() const { - if (!prior_graph_) { +void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const { + if (!dsg) { return; } // Only redraw if renderer has changes or we haven't drawn yet. if (has_drawn_ && !renderer_->hasChange()) { - CLOG(3) << "[ChangeDetectorVisualizer] Prior graph already drawn and no changes detected, " + MLOG(3) << "[ChangeDetectorVisualizer] Prior graph already drawn and no changes detected, " "skipping draw"; return; } - CLOG(2) << "[ChangeDetectorVisualizer] Drawing prior graph"; + MLOG(2) << "[ChangeDetectorVisualizer] Drawing prior graph"; std_msgs::msg::Header header; header.frame_id = config.global_frame_name; header.stamp = nh_.now(); - renderer_->draw(header, *prior_graph_); - mesh_plugin_->draw(header, *prior_graph_); + renderer_->draw(header, *dsg); + mesh_plugin_->draw(header, *dsg); renderer_->clearChangeFlag(); has_drawn_ = true; } From 7de86947da95420cc10b27f0028f79f750c13fce Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 4 Feb 2026 11:51:39 -0500 Subject: [PATCH 07/27] Visualize removed objects as red boundingbox --- ...active_window_change_detector_visualizer.h | 26 ++++++++ ...tive_window_change_detector_visualizer.cpp | 65 +++++++++++++++++++ 2 files changed, 91 insertions(+) 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 index ff3bf77..ca804bc 100644 --- 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 @@ -44,11 +44,16 @@ #include #include #include +#include #include +#include #include +#include +#include #include "khronos/active_window/change_detection/active_window_change_detector.h" + namespace khronos { class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector::ActiveWindowCDSink { @@ -68,6 +73,12 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: //! 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.4f; } const config; explicit ActiveWindowChangeDetectorVisualizer(const Config& config, @@ -81,10 +92,25 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: private: void drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const; + void visualizeChangedObjects(const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_object_ids) 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 hydra::MarkerTracker 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); diff --git a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index 01673e4..5c936e8 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -42,6 +42,8 @@ #include #include +#include "khronos_ros/visualization/visualization_utils.h" + namespace khronos { namespace { @@ -53,6 +55,9 @@ static const auto registration = } +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config) { using namespace config; name("ActiveWindowChangeDetectorVisualizer"); @@ -60,6 +65,9 @@ void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config) { 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"); + // TODO(multy): add checks for the fields. } ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( @@ -68,10 +76,14 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( : 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; + // Initialize publishers + object_bbox_pub_ = + nh_.create_publisher("changed_object_bounding_boxes", config.queue_size); MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Initialized."; } @@ -86,6 +98,14 @@ void ActiveWindowChangeDetectorVisualizer::call( MLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; } MLOG(1) << " are removed"; + + // set stamps for all visualizations + stamp_ = nh_.now(); + stamp_is_set_ = true; + + // Visualize removed objects + visualizeChangedObjects(dsg, removed_object_ids); + stamp_is_set_ = false; } void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const { @@ -112,4 +132,49 @@ void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGrap has_drawn_ = true; } +void ActiveWindowChangeDetectorVisualizer::visualizeChangedObjects( + const DynamicSceneGraph::Ptr& dsg, + const std::vector& removed_object_ids) const { + if (object_bbox_pub_->get_subscription_count() == 0u) { + return; + } + + // Get all removed object bounding boxes form the scene graph attributes + std::vector removed_object_bboxes; + for (const auto& id : removed_object_ids) { + const auto& object_node = dsg->getNode(id); + const auto* khronos_attrs = object_node.tryAttributes(); + if (khronos_attrs) { + removed_object_bboxes.push_back(khronos_attrs->bounding_box); + } else { + MLOG(2) << "[ActiveWindowChangeDetectorVisualizer] Could not find KhronosObjectAttributes " + "for removed object " + << spark_dsg::NodeSymbol(id).str(); + } + } + + // draw red bounding boxes for removed objects + MarkerArray new_markers; + new_markers.markers.reserve(removed_object_bboxes.size()); + + size_t id = 0u; + std_msgs::msg::Header header; + header.frame_id = config.global_frame_name; + header.stamp = getStamp(); + for (const auto& bbox : removed_object_bboxes) { + if (bbox.isValid()) { + auto& marker = new_markers.markers.emplace_back( + setBoundingBox(bbox, Color(255, 0, 0, 255), header, config.bounding_box_line_width)); + marker.id = id++; + } + } + + MarkerArray msg; + object_bbox_tracker_.add(new_markers, msg); + object_bbox_tracker_.clearPrevious(header, msg); + if (!msg.markers.empty()) { + object_bbox_pub_->publish(msg); + } +} + } // namespace khronos From 8cdf1d6db42f3b363f607c1c8c9b9be861cc421d Mon Sep 17 00:00:00 2001 From: Multyxu Date: Sun, 8 Feb 2026 10:56:53 -0500 Subject: [PATCH 08/27] More option for viewing everfree slice --- ...active_window_change_detector_visualizer.h | 2 +- .../visualization/active_window_visualizer.h | 9 +++ ...tive_window_change_detector_visualizer.cpp | 8 +-- .../active_window_visualizer.cpp | 69 ++++++++++++------- 4 files changed, 59 insertions(+), 29 deletions(-) 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 index ca804bc..c07c7c4 100644 --- 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 @@ -78,7 +78,7 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: int queue_size = 10; //! Width in meters of lines indicating bounding boxes. - float bounding_box_line_width = 0.4f; + float bounding_box_line_width = 0.1f; } const config; explicit ActiveWindowChangeDetectorVisualizer(const Config& config, 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/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index 5c936e8..de5ba5b 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -93,12 +93,11 @@ void ActiveWindowChangeDetectorVisualizer::call( drawPriorGraph(dsg); // print out removed object ids - MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Object "; + MLOG(3) << "[ActiveWindowChangeDetectorVisualizer] Object "; for (const auto& id : removed_object_ids) { - MLOG(1) << spark_dsg::NodeSymbol(id).str() << ", "; + MLOG(3) << spark_dsg::NodeSymbol(id).str() << ", "; } - MLOG(1) << " are removed"; - + MLOG(3) << " are removed"; // set stamps for all visualizations stamp_ = nh_.now(); stamp_is_set_ = true; @@ -110,6 +109,7 @@ void ActiveWindowChangeDetectorVisualizer::call( void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const { if (!dsg) { + MLOG(2) << "[ChangeDetectorVisualizer] No prior graph provided, skipping draw"; return; } 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. + } } } } From fd9e2876a7732639fe5a2ac3643fc591baabbba5 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 2 Mar 2026 19:16:50 -0500 Subject: [PATCH 09/27] first version of icp --- khronos/CMakeLists.txt | 15 +++ khronos/cmake/KhronosInstall.cmake | 2 +- .../active_window_change_detector.h | 44 ++++++- .../khronos/utils/icp_registration_utils.h | 79 ++++++++++++ khronos/package.xml | 1 + .../active_window_change_detector.cpp | 119 +++++++++++++++++- khronos/src/utils/icp_registration_utils.cpp | 71 +++++++++++ khronos_ros/CMakeLists.txt | 7 +- .../active_window_change_detector_ros.h | 94 ++++++++++++++ ...active_window_change_detector_visualizer.h | 8 +- khronos_ros/package.xml | 1 + ...tive_window_change_detector_visualizer.cpp | 26 +++- 12 files changed, 453 insertions(+), 14 deletions(-) create mode 100644 khronos/include/khronos/utils/icp_registration_utils.h create mode 100644 khronos/src/utils/icp_registration_utils.cpp create mode 100644 khronos_ros/include/khronos_ros/active_window/active_window_change_detector_ros.h diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index 3f59e25..6590628 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(OpenCV REQUIRED COMPONENTS core imgproc) find_package(spark_dsg REQUIRED) find_package(kimera_pgmo REQUIRED) find_package(hydra REQUIRED) +find_package(small_gicp REQUIRED) include(GNUInstallDirs) @@ -26,6 +27,19 @@ 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 @@ -76,6 +90,7 @@ ${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) 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 index d3b3cdb..40cc3ae 100644 --- 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 @@ -38,19 +38,24 @@ #pragma once #include +#include +#include #include #include #include #include "khronos/active_window/active_window.h" +#include "khronos/utils/icp_registration_utils.h" namespace khronos { class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { public: using ActiveWindowCDSink = - hydra::OutputSink&>; + hydra::OutputSink&, + const Eigen::Isometry3d&>; // Config. struct Config : hydra::VerbosityConfig { @@ -69,6 +74,22 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! Sinks for the change detector output. std::vector awcd_sinks; + + //! Enable ICP refinement on roman loop closure events. + 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. @@ -114,7 +135,13 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { * @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); + void setCurrentToPriorTransform(const Eigen::Isometry3d& current_T_prior) const; + + /** + * @brief Called from ROS subscriber thread on each ROMAN loop closure. + * Stores initial guess; consumed on next call(). + */ + void notifyLoopClosure(const Eigen::Isometry3d& current_T_prior_initial) const; /** * @brief Transform a point from prior map frame to current map frame. @@ -124,12 +151,23 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { Point transformPriorToCurrentFrame(const Eigen::Vector3d& point_in_prior) const; private: + /// 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; + //! 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 - Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); + mutable Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); + + //! Mutex protecting pending_lc_guess_ across threads. + mutable std::mutex lc_mutex_; + + //! Pending loop closure initial guess, set by notifyLoopClosure(), consumed in call(). + mutable std::optional pending_lc_guess_; //! Sinks for the change detector output. ActiveWindowCDSink::List sinks_; 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/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/change_detection/active_window_change_detector.cpp b/khronos/src/active_window/change_detection/active_window_change_detector.cpp index 8669013..fa71f7f 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -41,6 +41,8 @@ #include #include +#include "khronos/utils/icp_registration_utils.h" + namespace khronos { namespace { @@ -57,6 +59,13 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { field(config.removal_vertex_free_ratio_threshold, "removal_vertex_free_ratio_threshold"); field(config.prior_map_path, "prior_map_path"); field(config.awcd_sinks, "awcd_sinks"); + 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. @@ -70,6 +79,8 @@ ActiveWindowChangeDetector::ActiveWindowChangeDetector(const Config& config) 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) { @@ -81,6 +92,24 @@ void ActiveWindowChangeDetector::addKhronosSink(const ActiveWindowCDSink::Ptr& s void ActiveWindowChangeDetector::call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const { + // Consume pending loop closure and run ICP refinement. + if (config.enable_icp_refinement) { + std::optional pending; + { + std::lock_guard lock(lc_mutex_); + pending = std::move(pending_lc_guess_); + pending_lc_guess_.reset(); + } + if (pending.has_value()) { + runIcpRefinement(data, map, pending.value()); + } + } + + // tf = prior_map_localizaer.tf_lookup("map", "odom") + // make a virtual class in change_detection folder, (a new header file) + // in the ros side + // look at tf_lookup in hydra_ros as reference. + // 1. Find all object nodes in the prior graph within current volumetric map bounds const auto objects_id_in_bounds = findPriorObjectsInMapBounds(map); @@ -134,11 +163,11 @@ void ActiveWindowChangeDetector::call(const FrameData& data, } // 5. TODO (multy): need ways to report the problem or even visualize it. - MLOG(1) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() + MLOG(2) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() << " removed objects out of " << objects_id_in_bounds.size() << " checked"; // 6. Call all sinks with the removed objects - ActiveWindowCDSink::callAll(sinks_, prior_graph_, removed_object_ids); + ActiveWindowCDSink::callAll(sinks_, prior_graph_, removed_object_ids, current_T_prior_); } void ActiveWindowChangeDetector::loadPriorMap() { @@ -185,14 +214,14 @@ std::vector ActiveWindowChangeDetector::findPriorObjectsInMap } } - MLOG(1) << "[ActiveWindowChangeDetector] Found " << objects_in_bounds.size() + MLOG(2) << "[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 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" @@ -209,4 +238,86 @@ Point ActiveWindowChangeDetector::transformPriorToCurrentFrame( return point_in_current.cast(); } +void ActiveWindowChangeDetector::notifyLoopClosure(const Eigen::Isometry3d& T) const{ + std::lock_guard lock(lc_mutex_); + pending_lc_guess_ = T; + + // replace the transformation with the loop closure result + if (!config.enable_icp_refinement) { + setCurrentToPriorTransform(T); + } else { + MLOG(1) << "[ActiveWindowChangeDetector] Loop closure received, ICP scheduled."; + } + +} + +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 eular 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/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_ros/CMakeLists.txt b/khronos_ros/CMakeLists.txt index 3515087..986417b 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,6 +31,7 @@ add_library( src/experiments/experiment_manager.cpp src/experiments/experiment_directory.cpp src/khronos_pipeline.cpp + src/active_window/active_window_change_detector_ros.cpp src/visualization/active_window_visualizer.cpp src/visualization/active_window_change_detector_visualizer.cpp src/visualization/visualization_utils.cpp @@ -39,7 +42,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 # @@ -69,5 +72,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_ros.h b/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_ros.h new file mode 100644 index 0000000..cc28fca --- /dev/null +++ b/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_ros.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/active_window_change_detector.h" + +namespace khronos { + +/** + * @brief ROS-aware subclass of ActiveWindowChangeDetector that uses TF2 to obtain + * the current_T_prior initial guess automatically after hydra_multi processes a + * ROMAN loop closure and publishes the updated map→{robot}/odom transform. + * + * Loaded as a plugin into hydra_ros_node via `type: ActiveWindowChangeDetectorRos` + * in the khronos_sinks list. No modifications to existing pipeline code required. + */ +class ActiveWindowChangeDetectorRos : public ActiveWindowChangeDetector { + public: + struct Config : ActiveWindowChangeDetector::Config { + //! 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 re-triggering ICP. + double tf_change_threshold_m = 0.05; + } const config; + + explicit ActiveWindowChangeDetectorRos(const Config& config); + ~ActiveWindowChangeDetectorRos() override = default; + + /** + * @brief Looks up the latest map→robot_frame TF, checks for significant change, + * calls notifyLoopClosure() if needed, then invokes the parent call(). + */ + void call(const FrameData& data, + const VolumetricMap& map, + const Tracks& tracks) 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_tf_guess_ = Eigen::Isometry3d::Identity(); + mutable bool has_last_tf_ = false; +}; + +void declare_config(ActiveWindowChangeDetectorRos::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 index c07c7c4..89ebca7 100644 --- 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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -67,6 +68,9 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: //! 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; @@ -87,7 +91,8 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: // KhronosSink callback - called each frame. void call(const DynamicSceneGraph::Ptr& dsg, - const std::vector& removed_object_ids) const override; + const std::vector& removed_object_ids, + const Eigen::Isometry3d& current_T_prior) const override; private: void drawPriorGraph(const DynamicSceneGraph::Ptr& dsg) const; @@ -98,6 +103,7 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: // ROS ianvs::NodeHandle nh_; rclcpp::Publisher::SharedPtr object_bbox_pub_; + std::unique_ptr tf_broadcaster_; // Renderer and plugins std::shared_ptr renderer_; 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/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index de5ba5b..a14bc86 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include "khronos_ros/visualization/visualization_utils.h" @@ -84,12 +85,31 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( // Initialize publishers object_bbox_pub_ = nh_.create_publisher("changed_object_bounding_boxes", config.queue_size); + tf_broadcaster_ = std::make_unique(nh_.node()); MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Initialized."; } void ActiveWindowChangeDetectorVisualizer::call( const DynamicSceneGraph::Ptr& dsg, - const std::vector& removed_object_ids) const { + const std::vector& removed_object_ids, + const Eigen::Isometry3d& current_T_prior) const { + // Broadcast: {robot}/odom → prior_map_frame (config.global_frame_name) + // current_T_prior = odom_T_prior_map, so parent=odom, child=prior_map_frame. + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = nh_.now(); + tf_msg.header.frame_id = hydra::GlobalInfo::instance().getFrames().odom; + tf_msg.child_frame_id = config.global_frame_name; + const auto& t = current_T_prior.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(current_T_prior.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); + drawPriorGraph(dsg); // print out removed object ids @@ -99,7 +119,7 @@ void ActiveWindowChangeDetectorVisualizer::call( } MLOG(3) << " are removed"; // set stamps for all visualizations - stamp_ = nh_.now(); + stamp_ = rclcpp::Time(0); stamp_is_set_ = true; // Visualize removed objects @@ -124,7 +144,7 @@ void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGrap std_msgs::msg::Header header; header.frame_id = config.global_frame_name; - header.stamp = nh_.now(); + header.stamp = rclcpp::Time(0); renderer_->draw(header, *dsg); mesh_plugin_->draw(header, *dsg); From 0f35a0333a72c281a77d87b99579affed17ad768 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 2 Mar 2026 19:17:13 -0500 Subject: [PATCH 10/27] icp module --- .../active_window_change_detector_ros.cpp | 120 ++++++++++++++++++ 1 file changed, 120 insertions(+) create mode 100644 khronos_ros/src/active_window/active_window_change_detector_ros.cpp diff --git a/khronos_ros/src/active_window/active_window_change_detector_ros.cpp b/khronos_ros/src/active_window/active_window_change_detector_ros.cpp new file mode 100644 index 0000000..93328e7 --- /dev/null +++ b/khronos_ros/src/active_window/active_window_change_detector_ros.cpp @@ -0,0 +1,120 @@ +/** ----------------------------------------------------------------------------- + * 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_ros.h" + +#include +#include +#include +#include +#include +#include + +namespace khronos { +namespace { + +static const auto registration_ros = + config::RegistrationWithConfig( + "ActiveWindowChangeDetectorRos"); + +} // namespace + +void declare_config(ActiveWindowChangeDetectorRos::Config& config) { + using namespace config; + name("ActiveWindowChangeDetectorRos"); + // Declare all parent fields first. + base(config); + // New fields for this subclass. + 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"); +} + +ActiveWindowChangeDetectorRos::ActiveWindowChangeDetectorRos(const Config& cfg) + : ActiveWindowChangeDetector(cfg), config(cfg) { + auto nh = ianvs::NodeHandle::this_node(); + tf_buffer_ = std::make_shared(nh.clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + MLOG(1) << "[ActiveWindowChangeDetectorRos] TF listener created (" + << config.prior_frame_id << " -> " << getRobotFrame() << ")"; +} + +void ActiveWindowChangeDetectorRos::call(const FrameData& data, + const VolumetricMap& map, + const Tracks& tracks) 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 treated as "current"). + const Eigen::Isometry3d current_T_prior = map_T_odom.inverse(); + + const double delta = + (current_T_prior.translation() - last_tf_guess_.translation()).norm(); + if (!has_last_tf_ || delta > config.tf_change_threshold_m) { + MLOG(1) << "[ActiveWindowChangeDetectorRos] TF changed by " << delta + << " m, triggering ICP."; + notifyLoopClosure(current_T_prior); + last_tf_guess_ = current_T_prior; + has_last_tf_ = true; + } + } catch (const tf2::TransformException& e) { + MLOG(3) << "[ActiveWindowChangeDetectorRos] TF lookup failed: " << e.what(); + } + + ActiveWindowChangeDetector::call(data, map, tracks); +} + +// function that give map -> odom, in our case it's just tf lookup. + +std::string ActiveWindowChangeDetectorRos::getRobotFrame() const { + if (!config.robot_frame_id.empty()) { + return config.robot_frame_id; + } + return hydra::GlobalInfo::instance().getFrames().odom; +} + +} // namespace khronos From 6b1817abb757a7d96188a1562447e669b4fcdcf0 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 2 Mar 2026 20:23:57 -0500 Subject: [PATCH 11/27] remove active window change detector ros, and replace it with a get transform function through config uitilities creation. --- khronos/CMakeLists.txt | 1 + .../active_window_change_detector.h | 38 +++----- .../change_detection/transformation_getter.h | 92 +++++++++++++++++++ .../active_window_change_detector.cpp | 58 ++++-------- .../transformation_getter.cpp | 65 +++++++++++++ khronos_ros/CMakeLists.txt | 2 +- ...ector_ros.h => tf_transformation_getter.h} | 38 ++++---- ...r_ros.cpp => tf_transformation_getter.cpp} | 60 ++++++------ 8 files changed, 238 insertions(+), 116 deletions(-) create mode 100644 khronos/include/khronos/active_window/change_detection/transformation_getter.h create mode 100644 khronos/src/active_window/change_detection/transformation_getter.cpp rename khronos_ros/include/khronos_ros/active_window/{active_window_change_detector_ros.h => tf_transformation_getter.h} (69%) rename khronos_ros/src/active_window/{active_window_change_detector_ros.cpp => tf_transformation_getter.cpp} (65%) diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index 6590628..8dd0f4d 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(khronos::${PROJECT_NAME}_registration ALIAS ${PROJECT_NAME}_registra 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_buffer.cpp src/active_window/data/track.cpp src/active_window/integration/object_integrator.cpp 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 index 40cc3ae..2405e0b 100644 --- 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 @@ -38,7 +38,6 @@ #pragma once #include -#include #include #include @@ -46,23 +45,19 @@ #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: - using ActiveWindowCDSink = - hydra::OutputSink&, - const Eigen::Isometry3d&>; + using ActiveWindowCDSink = hydra::OutputSink&, + const Eigen::Isometry3d&>; // Config. struct Config : hydra::VerbosityConfig { - //! Verbosity level. - // int verbosity = hydra::GlobalInfo::instance().getConfig().default_verbosity; - // TODO(multy): after hydra is updated, should change it to also include a prefix like: "[Active - // Window Change Detector] " Config() : hydra::VerbosityConfig{hydra::GlobalInfo::instance().getConfig().default_verbosity} {} @@ -75,7 +70,11 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! Sinks for the change detector output. std::vector awcd_sinks; - //! Enable ICP refinement on roman loop closure events. + //! 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. @@ -137,12 +136,6 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { */ void setCurrentToPriorTransform(const Eigen::Isometry3d& current_T_prior) const; - /** - * @brief Called from ROS subscriber thread on each ROMAN loop closure. - * Stores initial guess; consumed on next call(). - */ - void notifyLoopClosure(const Eigen::Isometry3d& current_T_prior_initial) 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. @@ -150,12 +143,14 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { */ Point transformPriorToCurrentFrame(const Eigen::Vector3d& point_in_prior) const; - private: + 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, + void runIcpRefinement(const FrameData& data, + const VolumetricMap& map, const Eigen::Isometry3d& initial) const; + private: //! Prior map as a 3D scene graph. DynamicSceneGraph::Ptr prior_graph_; @@ -163,11 +158,8 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! current_point = current_T_prior_ * prior_point mutable Eigen::Isometry3d current_T_prior_ = Eigen::Isometry3d::Identity(); - //! Mutex protecting pending_lc_guess_ across threads. - mutable std::mutex lc_mutex_; - - //! Pending loop closure initial guess, set by notifyLoopClosure(), consumed in call(). - mutable std::optional pending_lc_guess_; + //! Plugin that provides the odom_T_prior transform. + TransformationGetter::Ptr transformation_getter_; //! Sinks for the change detector output. ActiveWindowCDSink::List sinks_; 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/src/active_window/change_detection/active_window_change_detector.cpp b/khronos/src/active_window/change_detection/active_window_change_detector.cpp index fa71f7f..f9ab378 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -59,6 +59,7 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { field(config.removal_vertex_free_ratio_threshold, "removal_vertex_free_ratio_threshold"); 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"); @@ -73,13 +74,14 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { 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 + // print out the config MLOG(1) << "[Khronos Active Window Change Detector] Config: " << config; } @@ -92,24 +94,16 @@ void ActiveWindowChangeDetector::addKhronosSink(const ActiveWindowCDSink::Ptr& s void ActiveWindowChangeDetector::call(const FrameData& data, const VolumetricMap& map, const Tracks& tracks) const { - // Consume pending loop closure and run ICP refinement. - if (config.enable_icp_refinement) { - std::optional pending; - { - std::lock_guard lock(lc_mutex_); - pending = std::move(pending_lc_guess_); - pending_lc_guess_.reset(); - } - if (pending.has_value()) { - runIcpRefinement(data, map, pending.value()); + // 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()); } } - // tf = prior_map_localizaer.tf_lookup("map", "odom") - // make a virtual class in change_detection folder, (a new header file) - // in the ros side - // look at tf_lookup in hydra_ros as reference. - // 1. Find all object nodes in the prior graph within current volumetric map bounds const auto objects_id_in_bounds = findPriorObjectsInMapBounds(map); @@ -238,19 +232,6 @@ Point ActiveWindowChangeDetector::transformPriorToCurrentFrame( return point_in_current.cast(); } -void ActiveWindowChangeDetector::notifyLoopClosure(const Eigen::Isometry3d& T) const{ - std::lock_guard lock(lc_mutex_); - pending_lc_guess_ = T; - - // replace the transformation with the loop closure result - if (!config.enable_icp_refinement) { - setCurrentToPriorTransform(T); - } else { - MLOG(1) << "[ActiveWindowChangeDetector] Loop closure received, ICP scheduled."; - } - -} - void ActiveWindowChangeDetector::runIcpRefinement(const FrameData& data, const VolumetricMap& map, const Eigen::Isometry3d& initial) const { @@ -289,11 +270,12 @@ void ActiveWindowChangeDetector::runIcpRefinement(const FrameData& data, 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); + 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 @@ -306,18 +288,16 @@ void ActiveWindowChangeDetector::runIcpRefinement(const FrameData& data, << ", translation delta: " << (current_T_prior_.translation() - initial.translation()).norm() << " m."; - // print initial guess and refined transform for debugging in x,y,z and eular angles x,y,z + // 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(); + << 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(); + << 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_ros/CMakeLists.txt b/khronos_ros/CMakeLists.txt index 986417b..9cc5778 100644 --- a/khronos_ros/CMakeLists.txt +++ b/khronos_ros/CMakeLists.txt @@ -31,7 +31,7 @@ add_library( src/experiments/experiment_manager.cpp src/experiments/experiment_directory.cpp src/khronos_pipeline.cpp - src/active_window/active_window_change_detector_ros.cpp + src/active_window/tf_transformation_getter.cpp src/visualization/active_window_visualizer.cpp src/visualization/active_window_change_detector_visualizer.cpp src/visualization/visualization_utils.cpp diff --git a/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_ros.h b/khronos_ros/include/khronos_ros/active_window/tf_transformation_getter.h similarity index 69% rename from khronos_ros/include/khronos_ros/active_window/active_window_change_detector_ros.h rename to khronos_ros/include/khronos_ros/active_window/tf_transformation_getter.h index cc28fca..5de5823 100644 --- a/khronos_ros/include/khronos_ros/active_window/active_window_change_detector_ros.h +++ b/khronos_ros/include/khronos_ros/active_window/tf_transformation_getter.h @@ -43,40 +43,40 @@ #include #include -#include "khronos/active_window/change_detection/active_window_change_detector.h" +#include "khronos/active_window/change_detection/transformation_getter.h" namespace khronos { /** - * @brief ROS-aware subclass of ActiveWindowChangeDetector that uses TF2 to obtain - * the current_T_prior initial guess automatically after hydra_multi processes a - * ROMAN loop closure and publishes the updated map→{robot}/odom transform. + * @brief TransformationGetter that obtains odom_T_prior by querying TF2. * - * Loaded as a plugin into hydra_ros_node via `type: ActiveWindowChangeDetectorRos` - * in the khronos_sinks list. No modifications to existing pipeline code required. + * 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 ActiveWindowChangeDetectorRos : public ActiveWindowChangeDetector { +class TFTransformationGetter : public TransformationGetter { public: - struct Config : ActiveWindowChangeDetector::Config { + 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 re-triggering ICP. + //! Minimum translation change [m] before reporting a new transform. double tf_change_threshold_m = 0.05; } const config; - explicit ActiveWindowChangeDetectorRos(const Config& config); - ~ActiveWindowChangeDetectorRos() override = default; + explicit TFTransformationGetter(const Config& config); /** - * @brief Looks up the latest map→robot_frame TF, checks for significant change, - * calls notifyLoopClosure() if needed, then invokes the parent call(). + * @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. */ - void call(const FrameData& data, - const VolumetricMap& map, - const Tracks& tracks) const override; + std::optional getTransformation() const override; private: //! Returns robot_frame_id from config if set, otherwise GlobalInfo odom frame. @@ -85,10 +85,10 @@ class ActiveWindowChangeDetectorRos : public ActiveWindowChangeDetector { std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - mutable Eigen::Isometry3d last_tf_guess_ = Eigen::Isometry3d::Identity(); - mutable bool has_last_tf_ = false; + mutable Eigen::Isometry3d last_reported_ = Eigen::Isometry3d::Identity(); + mutable bool has_last_ = false; }; -void declare_config(ActiveWindowChangeDetectorRos::Config& config); +void declare_config(TFTransformationGetter::Config& config); } // namespace khronos diff --git a/khronos_ros/src/active_window/active_window_change_detector_ros.cpp b/khronos_ros/src/active_window/tf_transformation_getter.cpp similarity index 65% rename from khronos_ros/src/active_window/active_window_change_detector_ros.cpp rename to khronos_ros/src/active_window/tf_transformation_getter.cpp index 93328e7..7b2ba3e 100644 --- a/khronos_ros/src/active_window/active_window_change_detector_ros.cpp +++ b/khronos_ros/src/active_window/tf_transformation_getter.cpp @@ -35,49 +35,44 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * -------------------------------------------------------------------------- */ -#include "khronos_ros/active_window/active_window_change_detector_ros.h" +#include "khronos_ros/active_window/tf_transformation_getter.h" #include -#include #include #include +#include #include #include namespace khronos { namespace { -static const auto registration_ros = - config::RegistrationWithConfig( - "ActiveWindowChangeDetectorRos"); +static const auto registration = + config::RegistrationWithConfig("TFTransformationGetter"); } // namespace -void declare_config(ActiveWindowChangeDetectorRos::Config& config) { +void declare_config(TFTransformationGetter::Config& config) { using namespace config; - name("ActiveWindowChangeDetectorRos"); - // Declare all parent fields first. - base(config); - // New fields for this subclass. + 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"); } -ActiveWindowChangeDetectorRos::ActiveWindowChangeDetectorRos(const Config& cfg) - : ActiveWindowChangeDetector(cfg), config(cfg) { +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) << "[ActiveWindowChangeDetectorRos] TF listener created (" - << config.prior_frame_id << " -> " << getRobotFrame() << ")"; + MLOG(1) << "[TFTransformationGetter] TF listener created (" << config.prior_frame_id << " -> " + << getRobotFrame() << ")"; } -void ActiveWindowChangeDetectorRos::call(const FrameData& data, - const VolumetricMap& map, - const Tracks& tracks) const { +std::optional TFTransformationGetter::getTransformation() const { try { const auto tf = tf_buffer_->lookupTransform(config.prior_frame_id, getRobotFrame(), tf2::TimePointZero); @@ -89,28 +84,25 @@ void ActiveWindowChangeDetectorRos::call(const FrameData& data, 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 treated as "current"). + // 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_tf_guess_.translation()).norm(); - if (!has_last_tf_ || delta > config.tf_change_threshold_m) { - MLOG(1) << "[ActiveWindowChangeDetectorRos] TF changed by " << delta - << " m, triggering ICP."; - notifyLoopClosure(current_T_prior); - last_tf_guess_ = current_T_prior; - has_last_tf_ = true; + 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) << "[ActiveWindowChangeDetectorRos] TF lookup failed: " << e.what(); + MLOG(3) << "[TFTransformationGetter] TF lookup failed: " << e.what(); + return std::nullopt; } - - ActiveWindowChangeDetector::call(data, map, tracks); } -// function that give map -> odom, in our case it's just tf lookup. - -std::string ActiveWindowChangeDetectorRos::getRobotFrame() const { +std::string TFTransformationGetter::getRobotFrame() const { if (!config.robot_frame_id.empty()) { return config.robot_frame_id; } From 4329230e1fa6b0941d72e620d994ae5877179c5e Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 23 Mar 2026 13:14:15 -0400 Subject: [PATCH 12/27] Change removed object detection logit to only consider points that are not unknown voxel. --- .../active_window_change_detector.h | 2 ++ .../active_window_change_detector.cpp | 27 ++++++++++++++++--- 2 files changed, 25 insertions(+), 4 deletions(-) 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 index 2405e0b..dab20e4 100644 --- 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 @@ -115,6 +115,8 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { 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. 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 index f9ab378..06de60d 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -125,27 +125,37 @@ void ActiveWindowChangeDetector::call(const FrameData& data, const auto& bbox = khronos_attrs->bounding_box; if (mesh.numVertices() == 0) { + MLOG(2) << "[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; - // 3. For each vertex, check if it's in free space + // 3. For each vertex in bound and not unknown, check if it's in free space 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 + } + + ++num_vertices_in_bound_and_known; + if (isPriorPointFree(vertex_current, map)) { ++num_vertices_in_free_space; } } - // 4. If more than threshold % of vertices are in free space, mark as removed - const float free_ratio = - static_cast(num_vertices_in_free_space) / static_cast(mesh.numVertices()); + // 4. Compute ratio of vertices in bound and known (?) in free space + const float free_ratio = static_cast(num_vertices_in_free_space) / + static_cast(num_vertices_in_bound_and_known); + // 5. If more than threshold % of vertices are in free space, mark as removed if (free_ratio >= config.removal_vertex_free_ratio_threshold) { removed_object_ids.push_back(object_id); MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() @@ -169,6 +179,9 @@ void ActiveWindowChangeDetector::loadPriorMap() { // 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, @@ -184,6 +197,12 @@ bool ActiveWindowChangeDetector::isPointInMapBounds(const Point& point, 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; From 33925c3e18db7166be4922298ebe6b8eaaad6ee4 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 12 May 2026 18:35:19 -0400 Subject: [PATCH 13/27] Add new boundingbox type, plan out new object addition. --- .../active_window_change_detector.h | 6 ++ .../active_window/tracking/max_iou_tracker.h | 2 + .../active_window_change_detector.cpp | 66 ++++++++++++++----- .../mesh_object_extractor.cpp | 2 +- .../tracking/max_iou_tracker.cpp | 6 ++ 5 files changed, 65 insertions(+), 17 deletions(-) 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 index dab20e4..e8389d5 100644 --- 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 @@ -111,6 +111,8 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { */ 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; @@ -152,6 +154,10 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { const VolumetricMap& map, const Eigen::Isometry3d& initial) const; + std::vector getRemovedObjects(const std::vector& objects_id_in_bounds, const VolumetricMap& map) const; + + std::vector getNewlyAddedObjects(const Tracks& tracks, const VolumetricMap& map) const; + private: //! Prior map as a 3D scene graph. DynamicSceneGraph::Ptr prior_graph_; 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..17eb464 100644 --- a/khronos/include/khronos/active_window/tracking/max_iou_tracker.h +++ b/khronos/include/khronos/active_window/tracking/max_iou_tracker.h @@ -90,6 +90,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. 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 index 06de60d..701911c 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -91,22 +91,7 @@ void ActiveWindowChangeDetector::addKhronosSink(const ActiveWindowCDSink::Ptr& s } } -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); - +std::vector ActiveWindowChangeDetector::getRemovedObjects(const std::vector& objects_id_in_bounds, const VolumetricMap& map) const { std::vector removed_object_ids; // 2. For each object node, get mesh vertices and transform to current frame @@ -170,6 +155,55 @@ void ActiveWindowChangeDetector::call(const FrameData& data, MLOG(2) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() << " removed objects out of " << objects_id_in_bounds.size() << " checked"; + return removed_object_ids; +} + +std::vector ActiveWindowChangeDetector::getNewlyAddedObjects(const Tracks& tracks, const VolumetricMap& map) const { + std::vector newly_added_object_ids; + + // 1. For each track, we need to get the bounding box or the voxel indices (probably should be 3D) in the current active window (some kind of volume) + + + // 2. Get all the prior traversability places from the piror map within the current active window bounds. + // 3. Find voxels 2d indcies that is contained in the prior traversability places, meaning those voxels are known to be free in the prior map. + // (maybe need some polygon containment check.) + // 4. Then, we can compute the IoU between each track's voxels and the prior traversability places voxels. + // 5. If the IoU is smaller than certain threshold, we can consider this track to be newly added. (new occupancy in the previous free space in the prior map). + // 6. We store the newly added objects' track id in a list + + return newly_added_object_ids; +} + +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); + + const auto removed_object_ids = getRemovedObjects(objects_id_in_bounds, map); + + + // Get newly added objects + const auto newly_added_object_ids = getNewlyAddedObjects(tracks, map); + + // Note on known limitation: we can't get added objects on the table + + // If tracking, we then just need to compare track with the object node in the prior map. Similar to place layer, we compute IoU of the voxels. + + // need to pass the newly added track to the visualizatio sink. + + // compte the confidence based on the IoU is nice, and we also probably want to have way to visualize the confidence. + // 6. Call all sinks with the removed objects ActiveWindowCDSink::callAll(sinks_, prior_graph_, removed_object_ids, current_T_prior_); } 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..958748b 100644 --- a/khronos/src/active_window/object_extraction/mesh_object_extractor.cpp +++ b/khronos/src/active_window/object_extraction/mesh_object_extractor.cpp @@ -287,7 +287,7 @@ KhronosObjectAttributes::Ptr MeshObjectExtractor::extractStaticObject( if (object->mesh.points.empty()) { object->bounding_box = extent; } else { - object->bounding_box = BoundingBox(object->mesh.points); + object->bounding_box = BoundingBox(object->mesh.points); // TODO change bounding box type to AABB or RAABB } if (object->bounding_box.volume() > config.max_object_volume) { CLOG(5) << "[MeshObjectExtractor] Dropping " << getTrackName(track) << ": large volume (" diff --git a/khronos/src/active_window/tracking/max_iou_tracker.cpp b/khronos/src/active_window/tracking/max_iou_tracker.cpp index 45bf3db..0512a97 100644 --- a/khronos/src/active_window/tracking/max_iou_tracker.cpp +++ b/khronos/src/active_window/tracking/max_iou_tracker.cpp @@ -139,12 +139,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"); @@ -434,6 +436,10 @@ void MaxIoUTracker::setupTrackMeasurements(FrameData& data) const { setupTrackMeasurement(data, cluster); // NOTE(lschmid): Compute the bounding boxes for all clusters for visualization and extent // computation in the future. + + // add config to choose different type of bbox + // cluster.bounding_box = + // BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map), BoundingBox::Type::RAABB); cluster.bounding_box = BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map)); } From 2d87a1a6a01c63e35cfeb92b4d0fd420117c2448 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Sun, 14 Jun 2026 10:40:22 -0400 Subject: [PATCH 14/27] Detect added objects. Adding option for rotated bbox. - But, RAABB does not do merge well - Per frame add/detect vis is noisy. --- .../active_window_change_detector.h | 17 +- .../object_extraction/mesh_object_extractor.h | 3 + .../active_window_change_detector.cpp | 181 ++++++++++++++---- .../mesh_object_extractor.cpp | 6 +- .../tracking/max_iou_tracker.cpp | 11 +- ...active_window_change_detector_visualizer.h | 6 + ...tive_window_change_detector_visualizer.cpp | 39 ++++ 7 files changed, 223 insertions(+), 40 deletions(-) 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 index e8389d5..91a3b80 100644 --- 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 @@ -54,6 +54,7 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { public: using ActiveWindowCDSink = hydra::OutputSink&, + const std::vector&, const Eigen::Isometry3d&>; // Config. @@ -67,6 +68,10 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! Ratio of free prior map points of an object's vertices to consider it removed. float removal_vertex_free_ratio_threshold = 0.8f; + //! Min fraction of a track's 2D footprint that must lie in prior traversable + //! (free) space to classify the track as a newly-added object. + float added_object_containment_threshold = 0.5f; + //! Sinks for the change detector output. std::vector awcd_sinks; @@ -155,9 +160,19 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { const Eigen::Isometry3d& initial) const; std::vector getRemovedObjects(const std::vector& objects_id_in_bounds, const VolumetricMap& map) const; - + std::vector getNewlyAddedObjects(const Tracks& tracks, const VolumetricMap& map) 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_; 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/src/active_window/change_detection/active_window_change_detector.cpp b/khronos/src/active_window/change_detection/active_window_change_detector.cpp index 701911c..017052b 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include "khronos/utils/icp_registration_utils.h" @@ -57,6 +58,7 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { name("ActiveWindowChangeDetector"); field(config.verbosity, "verbosity"); field(config.removal_vertex_free_ratio_threshold, "removal_vertex_free_ratio_threshold"); + field(config.added_object_containment_threshold, "added_object_containment_threshold"); field(config.prior_map_path, "prior_map_path"); field(config.awcd_sinks, "awcd_sinks"); field(config.transformation_getter, "transformation_getter"); @@ -101,7 +103,7 @@ std::vector ActiveWindowChangeDetector::getRemovedObjects(con // Cast to KhronosObjectAttributes to access mesh const auto* khronos_attrs = object_node.tryAttributes(); if (!khronos_attrs) { - MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(5) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " does not have KhronosObjectAttributes, skipping"; continue; } @@ -110,7 +112,7 @@ std::vector ActiveWindowChangeDetector::getRemovedObjects(con const auto& bbox = khronos_attrs->bounding_box; if (mesh.numVertices() == 0) { - MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(5) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " has empty mesh, skipping"; continue; } @@ -143,35 +145,142 @@ std::vector ActiveWindowChangeDetector::getRemovedObjects(con // 5. If more than threshold % of vertices are in free space, mark as removed if (free_ratio >= config.removal_vertex_free_ratio_threshold) { removed_object_ids.push_back(object_id); - MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " detected as REMOVED (free ratio: " << free_ratio << ")"; } else { - MLOG(2) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() << " still present (free ratio: " << free_ratio << ")"; } } - // 5. TODO (multy): need ways to report the problem or even visualize it. - MLOG(2) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() + MLOG(4) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() << " removed objects out of " << objects_id_in_bounds.size() << " checked"; - return removed_object_ids; + return removed_object_ids; } -std::vector ActiveWindowChangeDetector::getNewlyAddedObjects(const Tracks& tracks, const VolumetricMap& map) const { - std::vector newly_added_object_ids; +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; +} - // 1. For each track, we need to get the bounding box or the voxel indices (probably should be 3D) in the current active window (some kind of volume) +std::vector ActiveWindowChangeDetector::getNewlyAddedObjects(const Tracks& tracks, + const VolumetricMap& map) const { + std::vector newly_added_track_ids; - - // 2. Get all the prior traversability places from the piror map within the current active window bounds. - // 3. Find voxels 2d indcies that is contained in the prior traversability places, meaning those voxels are known to be free in the prior map. - // (maybe need some polygon containment check.) - // 4. Then, we can compute the IoU between each track's voxels and the prior traversability places voxels. - // 5. If the IoU is smaller than certain threshold, we can consider this track to be newly added. (new occupancy in the previous free space in the prior map). - // 6. We store the newly added objects' track id in a list + // Build the 2D free-space footprint from prior traversability places. + const GlobalIndexSet prior_free = getPriorFreeFootprint2D(map); + if (prior_free.empty()) { + return newly_added_track_ids; + } + + 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()); - return newly_added_object_ids; + if (containment >= config.added_object_containment_threshold) { + newly_added_track_ids.push_back(track.id); + MLOG(3) << "[ActiveWindowChangeDetector] Track " << track.id + << " detected as ADDED (containment: " << containment << ")"; + } else { + MLOG(3) << "[ActiveWindowChangeDetector] Track " << track.id + << " not newly added (containment: " << containment << ")"; + } + } + + MLOG(2) << "[ActiveWindowChangeDetector] Detected " << newly_added_track_ids.size() + << " newly added objects out of " << tracks.size() << " tracks checked"; + + return newly_added_track_ids; } void ActiveWindowChangeDetector::call(const FrameData& data, @@ -192,20 +301,28 @@ void ActiveWindowChangeDetector::call(const FrameData& data, const auto removed_object_ids = getRemovedObjects(objects_id_in_bounds, map); + // Detect newly added objects (tracks whose footprint sits on prior-traversable ground). + // Known limitation: objects on tables cannot be detected this way. + const auto newly_added_ids = getNewlyAddedObjects(tracks, map); + + // Build the Track objects for newly-added detections so sinks can access bbox / confidence. + std::vector newly_added_tracks; + newly_added_tracks.reserve(newly_added_ids.size()); + for (int id : newly_added_ids) { + for (const Track& t : tracks) { + if (t.id == id) { + newly_added_tracks.push_back(t); + break; + } + } + } - // Get newly added objects - const auto newly_added_object_ids = getNewlyAddedObjects(tracks, map); - - // Note on known limitation: we can't get added objects on the table - - // If tracking, we then just need to compare track with the object node in the prior map. Similar to place layer, we compute IoU of the voxels. - - // need to pass the newly added track to the visualizatio sink. - - // compte the confidence based on the IoU is nice, and we also probably want to have way to visualize the confidence. + MLOG(2) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() << " removed objects and " + << newly_added_tracks.size() << " newly added tracks."; - // 6. Call all sinks with the removed objects - ActiveWindowCDSink::callAll(sinks_, prior_graph_, removed_object_ids, current_T_prior_); + // Call all sinks with removed objects, newly-added tracks, and the prior-to-current transform. + ActiveWindowCDSink::callAll( + sinks_, prior_graph_, removed_object_ids, newly_added_tracks, current_T_prior_); } void ActiveWindowChangeDetector::loadPriorMap() { @@ -255,13 +372,13 @@ std::vector ActiveWindowChangeDetector::findPriorObjectsInMap if (isPointInMapBounds(position_in_current, map)) { objects_in_bounds.push_back(node_id); - MLOG(3) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(node_id).str() + MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(node_id).str() << " is within map bounds at position (current frame): " << position_in_current.transpose(); } } - MLOG(2) << "[ActiveWindowChangeDetector] Found " << objects_in_bounds.size() + MLOG(4) << "[ActiveWindowChangeDetector] Found " << objects_in_bounds.size() << " prior objects within current map bounds"; return objects_in_bounds; 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 958748b..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); // TODO change bounding box type to AABB or RAABB + 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/tracking/max_iou_tracker.cpp b/khronos/src/active_window/tracking/max_iou_tracker.cpp index 0512a97..e22cd94 100644 --- a/khronos/src/active_window/tracking/max_iou_tracker.cpp +++ b/khronos/src/active_window/tracking/max_iou_tracker.cpp @@ -432,21 +432,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. - - // add config to choose different type of bbox - // cluster.bounding_box = - // BoundingBox(utils::VertexMapAdaptor(cluster.pixels, data.input.vertex_map), BoundingBox::Type::RAABB); 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); } } 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 index 89ebca7..e9092ea 100644 --- 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 @@ -53,6 +53,7 @@ #include #include "khronos/active_window/change_detection/active_window_change_detector.h" +#include "khronos/active_window/data/track.h" namespace khronos { @@ -92,6 +93,7 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: // KhronosSink callback - called each frame. void call(const DynamicSceneGraph::Ptr& dsg, const std::vector& removed_object_ids, + const std::vector& newly_added_tracks, const Eigen::Isometry3d& current_T_prior) const override; private: @@ -100,6 +102,9 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: void visualizeChangedObjects(const DynamicSceneGraph::Ptr& dsg, const std::vector& removed_object_ids) 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_; @@ -114,6 +119,7 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: mutable rclcpp::Time stamp_; mutable bool stamp_is_set_ = false; 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(); } diff --git a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index a14bc86..31d792c 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -92,6 +92,7 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( void ActiveWindowChangeDetectorVisualizer::call( const DynamicSceneGraph::Ptr& dsg, const std::vector& removed_object_ids, + const std::vector& newly_added_tracks, const Eigen::Isometry3d& current_T_prior) const { // Broadcast: {robot}/odom → prior_map_frame (config.global_frame_name) // current_T_prior = odom_T_prior_map, so parent=odom, child=prior_map_frame. @@ -124,6 +125,7 @@ void ActiveWindowChangeDetectorVisualizer::call( // Visualize removed objects visualizeChangedObjects(dsg, removed_object_ids); + visualizeAddedObjects(newly_added_tracks, current_T_prior); stamp_is_set_ = false; } @@ -197,4 +199,41 @@ void ActiveWindowChangeDetectorVisualizer::visualizeChangedObjects( } } +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 prior_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()); + + size_t id = 0u; + 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)); + marker.id = 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 From 5b500b7bb9c2b9c4c73d76be602ed47f24107b2f Mon Sep 17 00:00:00 2001 From: Multyxu Date: Sun, 14 Jun 2026 12:02:20 -0400 Subject: [PATCH 15/27] Persistent removed object bounding box. --- .../active_window_change_detector.h | 51 ++++++++++- .../active_window_change_detector.cpp | 85 +++++++++++++++---- 2 files changed, 118 insertions(+), 18 deletions(-) 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 index 91a3b80..38a677f 100644 --- 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 @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -52,6 +53,19 @@ 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; + }; + using ActiveWindowCDSink = hydra::OutputSink&, const std::vector&, @@ -65,9 +79,22 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! 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 to consider it removed. + //! 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; + //! Min fraction of a track's 2D footprint that must lie in prior traversable //! (free) space to classify the track as a newly-added object. float added_object_containment_threshold = 0.5f; @@ -159,7 +186,23 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { const VolumetricMap& map, const Eigen::Isometry3d& initial) const; - std::vector getRemovedObjects(const std::vector& objects_id_in_bounds, const VolumetricMap& map) 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). + */ + std::vector updateRemovedFilter( + const std::unordered_map& measurements) const; std::vector getNewlyAddedObjects(const Tracks& tracks, const VolumetricMap& map) const; @@ -181,6 +224,10 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! 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_; + //! Plugin that provides the odom_T_prior transform. TransformationGetter::Ptr transformation_getter_; 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 index 017052b..db5ebce 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -58,6 +58,9 @@ void declare_config(ActiveWindowChangeDetector::Config& 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.prior_map_path, "prior_map_path"); field(config.awcd_sinks, "awcd_sinks"); @@ -93,14 +96,14 @@ void ActiveWindowChangeDetector::addKhronosSink(const ActiveWindowCDSink::Ptr& s } } -std::vector ActiveWindowChangeDetector::getRemovedObjects(const std::vector& objects_id_in_bounds, const VolumetricMap& map) const { - std::vector removed_object_ids; +std::unordered_map ActiveWindowChangeDetector::computeFreeRatios( + const std::vector& objects_id_in_bounds, + const VolumetricMap& map) const { + std::unordered_map measurements; - // 2. For each object node, get mesh vertices and transform to current frame for (const auto object_id : objects_id_in_bounds) { const auto& object_node = prior_graph_->getNode(object_id); - // Cast to KhronosObjectAttributes to access mesh const auto* khronos_attrs = object_node.tryAttributes(); if (!khronos_attrs) { MLOG(5) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() @@ -120,7 +123,6 @@ std::vector ActiveWindowChangeDetector::getRemovedObjects(con int num_vertices_in_free_space = 0; int num_vertices_in_bound_and_known = 0; - // 3. For each vertex in bound and not unknown, check if it's in free space for (size_t i = 0; i < mesh.numVertices(); ++i) { // Transform: local → prior_world → current_world const Eigen::Vector3f vertex_local = mesh.pos(i); @@ -128,7 +130,7 @@ std::vector ActiveWindowChangeDetector::getRemovedObjects(con const Point vertex_current = transformPriorToCurrentFrame(vertex_prior_world.cast()); if (!isPointKnown(vertex_current, map)) { - continue; // Skip unknown points + continue; // Skip unknown points — no valid measurement at this vertex. } ++num_vertices_in_bound_and_known; @@ -138,23 +140,70 @@ std::vector ActiveWindowChangeDetector::getRemovedObjects(con } } - // 4. Compute ratio of vertices in bound and known (?) 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); - // 5. If more than threshold % of vertices are in free space, mark as removed - if (free_ratio >= config.removal_vertex_free_ratio_threshold) { + 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) 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_object_ids; + for (const 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) { removed_object_ids.push_back(object_id); - MLOG(4) << "[ActiveWindowChangeDetector] Object " << spark_dsg::NodeSymbol(object_id).str() - << " detected as REMOVED (free ratio: " << free_ratio << ")"; + 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() - << " still present (free ratio: " << free_ratio << ")"; + << " not removed (p=" << state.free_probability + << ", frames=" << state.num_frames_observed << ")"; } } - MLOG(4) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.size() - << " removed objects out of " << objects_id_in_bounds.size() << " checked"; + MLOG(2) << "[ActiveWindowChangeDetector] Removed-object filter: " + << removed_object_ids.size() << " removed out of " + << removed_object_states_.size() << " tracked candidates"; return removed_object_ids; } @@ -296,10 +345,14 @@ void ActiveWindowChangeDetector::call(const FrameData& data, } } - // 1. Find all object nodes in the prior graph within current volumetric map bounds + // 1. Find all object nodes in the prior graph within current volumetric map bounds. const auto objects_id_in_bounds = findPriorObjectsInMapBounds(map); - const auto removed_object_ids = getRemovedObjects(objects_id_in_bounds, 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_object_ids = updateRemovedFilter(free_ratio_measurements); // Detect newly added objects (tracks whose footprint sits on prior-traversable ground). // Known limitation: objects on tables cannot be detected this way. From 064d156c54f74c2f646b9b81d731f12bc6814ae1 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 15 Jun 2026 10:09:46 -0400 Subject: [PATCH 16/27] Added track probability for more stable visualization. --- .../active_window_change_detector.h | 70 +++++++++- .../active_window_change_detector.cpp | 124 ++++++++++++++---- 2 files changed, 162 insertions(+), 32 deletions(-) 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 index 38a677f..2442048 100644 --- 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 @@ -66,6 +66,27 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { float last_free_ratio = 0.0f; }; + /** + * @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&, @@ -95,10 +116,24 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! Guards against single-frame spikes on newly-entered objects. int removal_min_frames_observed = 3; - //! Min fraction of a track's 2D footprint that must lie in prior traversable - //! (free) space to classify the track as a newly-added object. + //! 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; @@ -108,19 +143,26 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! 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. @@ -204,7 +246,22 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { std::vector updateRemovedFilter( const std::unordered_map& measurements) const; - std::vector getNewlyAddedObjects(const Tracks& tracks, const VolumetricMap& map) 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. @@ -228,6 +285,13 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { //! 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_; 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 index db5ebce..ac3fc0a 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -62,6 +62,10 @@ void declare_config(ActiveWindowChangeDetector::Config& config) { 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"); @@ -284,14 +288,15 @@ GlobalIndexSet ActiveWindowChangeDetector::getTrackFootprint2D(const Track& trac return footprint; } -std::vector ActiveWindowChangeDetector::getNewlyAddedObjects(const Tracks& tracks, - const VolumetricMap& map) const { - std::vector newly_added_track_ids; +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 newly_added_track_ids; + return measurements; } const float voxel_size = map.config.voxel_size; @@ -316,20 +321,90 @@ std::vector ActiveWindowChangeDetector::getNewlyAddedObjects(const Tracks& } const float containment = static_cast(intersection) / static_cast(track2D.size()); - if (containment >= config.added_object_containment_threshold) { - newly_added_track_ids.push_back(track.id); - MLOG(3) << "[ActiveWindowChangeDetector] Track " << track.id - << " detected as ADDED (containment: " << containment << ")"; + 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 { - MLOG(3) << "[ActiveWindowChangeDetector] Track " << track.id - << " not newly added (containment: " << containment << ")"; + // 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; } - MLOG(2) << "[ActiveWindowChangeDetector] Detected " << newly_added_track_ids.size() - << " newly added objects out of " << tracks.size() << " tracks checked"; + // 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; - return newly_added_track_ids; + 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, @@ -354,24 +429,15 @@ void ActiveWindowChangeDetector::call(const FrameData& data, // 3. Update EMA filter and threshold to obtain the temporally-filtered removed set. const auto removed_object_ids = updateRemovedFilter(free_ratio_measurements); - // Detect newly added objects (tracks whose footprint sits on prior-traversable ground). - // Known limitation: objects on tables cannot be detected this way. - const auto newly_added_ids = getNewlyAddedObjects(tracks, map); + // 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); - // Build the Track objects for newly-added detections so sinks can access bbox / confidence. - std::vector newly_added_tracks; - newly_added_tracks.reserve(newly_added_ids.size()); - for (int id : newly_added_ids) { - for (const Track& t : tracks) { - if (t.id == id) { - newly_added_tracks.push_back(t); - break; - } - } - } + // 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_object_ids.size() << " removed objects and " - << newly_added_tracks.size() << " newly added tracks."; + MLOG(2) << "[ActiveWindowChangeDetector] Detected " << removed_object_ids.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( From f024bf87f84f840e4c99f916365087239e20ad28 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 15 Jun 2026 11:01:56 -0400 Subject: [PATCH 17/27] Solving visualization flickering --- ...active_window_change_detector_visualizer.h | 5 ++ ...tive_window_change_detector_visualizer.cpp | 52 +++++++++++-------- 2 files changed, 34 insertions(+), 23 deletions(-) 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 index e9092ea..09f7dd0 100644 --- 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 @@ -38,6 +38,7 @@ #pragma once #include +#include #include #include @@ -84,6 +85,9 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: //! 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, @@ -118,6 +122,7 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: 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_; diff --git a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index 31d792c..0800f16 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -68,6 +68,7 @@ void declare_config(ActiveWindowChangeDetectorVisualizer::Config& config) { 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. } @@ -81,6 +82,7 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( 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_ = @@ -111,6 +113,15 @@ void ActiveWindowChangeDetectorVisualizer::call( tf_msg.transform.rotation.w = q.w(); tf_broadcaster_->sendTransform(tf_msg); + // 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 @@ -161,34 +172,27 @@ void ActiveWindowChangeDetectorVisualizer::visualizeChangedObjects( return; } - // Get all removed object bounding boxes form the scene graph attributes - std::vector removed_object_bboxes; - for (const auto& id : removed_object_ids) { - const auto& object_node = dsg->getNode(id); - const auto* khronos_attrs = object_node.tryAttributes(); - if (khronos_attrs) { - removed_object_bboxes.push_back(khronos_attrs->bounding_box); - } else { - MLOG(2) << "[ActiveWindowChangeDetectorVisualizer] Could not find KhronosObjectAttributes " - "for removed object " - << spark_dsg::NodeSymbol(id).str(); - } - } - // draw red bounding boxes for removed objects MarkerArray new_markers; - new_markers.markers.reserve(removed_object_bboxes.size()); + new_markers.markers.reserve(removed_object_ids.size()); - size_t id = 0u; std_msgs::msg::Header header; header.frame_id = config.global_frame_name; header.stamp = getStamp(); - for (const auto& bbox : removed_object_bboxes) { - if (bbox.isValid()) { - auto& marker = new_markers.markers.emplace_back( - setBoundingBox(bbox, Color(255, 0, 0, 255), header, config.bounding_box_line_width)); - marker.id = id++; + for (size_t i = 0u; i < removed_object_ids.size(); ++i) { + const auto& id = removed_object_ids[i]; + const auto& object_node = dsg->getNode(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(id & 0xffffffff); } MarkerArray msg; @@ -212,7 +216,6 @@ void ActiveWindowChangeDetectorVisualizer::visualizeAddedObjects( MarkerArray new_markers; new_markers.markers.reserve(newly_added_tracks.size()); - size_t id = 0u; std_msgs::msg::Header header; header.frame_id = config.global_frame_name; header.stamp = getStamp(); @@ -225,7 +228,10 @@ void ActiveWindowChangeDetectorVisualizer::visualizeAddedObjects( 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)); - marker.id = id++; + // 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; From 61481ccf168e6e24d64bfa540975a8945b61a7e4 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Mon, 15 Jun 2026 18:25:27 -0400 Subject: [PATCH 18/27] Change tf frame order, add a new frame in the midel pre_icp_odom --- khronos_ros/CMakeLists.txt | 1 + .../active_window/tf_icp_publisher.h | 99 +++++++++++++ ...active_window_change_detector_visualizer.h | 3 - .../src/active_window/tf_icp_publisher.cpp | 131 ++++++++++++++++++ ...tive_window_change_detector_visualizer.cpp | 21 +-- 5 files changed, 232 insertions(+), 23 deletions(-) create mode 100644 khronos_ros/include/khronos_ros/active_window/tf_icp_publisher.h create mode 100644 khronos_ros/src/active_window/tf_icp_publisher.cpp diff --git a/khronos_ros/CMakeLists.txt b/khronos_ros/CMakeLists.txt index 9cc5778..c425f4a 100644 --- a/khronos_ros/CMakeLists.txt +++ b/khronos_ros/CMakeLists.txt @@ -32,6 +32,7 @@ add_library( src/experiments/experiment_directory.cpp src/khronos_pipeline.cpp src/active_window/tf_transformation_getter.cpp + src/active_window/tf_icp_publisher.cpp src/visualization/active_window_visualizer.cpp src/visualization/active_window_change_detector_visualizer.cpp src/visualization/visualization_utils.cpp 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..2f1835e --- /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_object_ids, + 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/visualization/active_window_change_detector_visualizer.h b/khronos_ros/include/khronos_ros/visualization/active_window_change_detector_visualizer.h index 09f7dd0..183b533 100644 --- 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 @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -112,8 +111,6 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: // ROS ianvs::NodeHandle nh_; rclcpp::Publisher::SharedPtr object_bbox_pub_; - std::unique_ptr tf_broadcaster_; - // Renderer and plugins std::shared_ptr renderer_; std::shared_ptr mesh_plugin_; 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..b2fe781 --- /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_object_ids*/, + 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/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index 0800f16..4cfa8f4 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include "khronos_ros/visualization/visualization_utils.h" @@ -87,7 +86,6 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( // Initialize publishers object_bbox_pub_ = nh_.create_publisher("changed_object_bounding_boxes", config.queue_size); - tf_broadcaster_ = std::make_unique(nh_.node()); MLOG(1) << "[ActiveWindowChangeDetectorVisualizer] Initialized."; } @@ -96,23 +94,6 @@ void ActiveWindowChangeDetectorVisualizer::call( const std::vector& removed_object_ids, const std::vector& newly_added_tracks, const Eigen::Isometry3d& current_T_prior) const { - // Broadcast: {robot}/odom → prior_map_frame (config.global_frame_name) - // current_T_prior = odom_T_prior_map, so parent=odom, child=prior_map_frame. - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = nh_.now(); - tf_msg.header.frame_id = hydra::GlobalInfo::instance().getFrames().odom; - tf_msg.child_frame_id = config.global_frame_name; - const auto& t = current_T_prior.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(current_T_prior.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); - // Throttle visualization redraws to reduce flicker from high-frequency calls. if (config.min_draw_period_s > 0.0) { const rclcpp::Time now = nh_.now(); @@ -210,7 +191,7 @@ void ActiveWindowChangeDetectorVisualizer::visualizeAddedObjects( return; } - // Transform track bounding boxes (in current/odom frame) into prior_map_frame for RViz. + // 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; From 837a78510f983e0d3fcb53794e35dbbf5759f15b Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 1 Jul 2026 18:21:28 -0400 Subject: [PATCH 19/27] Add new msg type for global msg back to base station. Add removed object timestamp at fisrt ovservation of absence. Change Sink to use the removed object type instead of id. --- .../active_window_change_detector.h | 26 ++- .../active_window_change_detector.cpp | 25 ++- khronos_msgs/CMakeLists.txt | 4 + khronos_msgs/msg/AwcdChanges.msg | 14 ++ khronos_msgs/msg/ChangedObjectInfo.msg | 23 +++ khronos_msgs/package.xml | 1 + khronos_ros/CMakeLists.txt | 1 + .../active_window_change_detector_publisher.h | 112 ++++++++++++ .../active_window/tf_icp_publisher.h | 2 +- ...active_window_change_detector_visualizer.h | 7 +- ...ctive_window_change_detector_publisher.cpp | 172 ++++++++++++++++++ .../src/active_window/tf_icp_publisher.cpp | 2 +- ...tive_window_change_detector_visualizer.cpp | 19 +- 13 files changed, 379 insertions(+), 29 deletions(-) create mode 100644 khronos_msgs/msg/AwcdChanges.msg create mode 100644 khronos_msgs/msg/ChangedObjectInfo.msg create mode 100644 khronos_ros/include/khronos_ros/active_window/active_window_change_detector_publisher.h create mode 100644 khronos_ros/src/active_window/active_window_change_detector_publisher.cpp 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 index 2442048..8b51f45 100644 --- 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 @@ -64,6 +64,20 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { 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; }; /** @@ -88,7 +102,7 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { }; using ActiveWindowCDSink = hydra::OutputSink&, + const std::vector&, const std::vector&, const Eigen::Isometry3d&>; @@ -241,10 +255,14 @@ class ActiveWindowChangeDetector : public ActiveWindow::KhronosSink { * @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). + * 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) const; + std::vector updateRemovedFilter( + const std::unordered_map& measurements, TimeStamp stamp) const; /** * @brief Compute the per-frame containment ratio for each eligible track (non-dynamic, 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 index ac3fc0a..88acea7 100644 --- a/khronos/src/active_window/change_detection/active_window_change_detector.cpp +++ b/khronos/src/active_window/change_detection/active_window_change_detector.cpp @@ -163,8 +163,8 @@ std::unordered_map ActiveWindowChangeDetector::compute return measurements; } -std::vector ActiveWindowChangeDetector::updateRemovedFilter( - const std::unordered_map& measurements) const { +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) { @@ -189,12 +189,17 @@ std::vector ActiveWindowChangeDetector::updateRemovedFilter( // 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_object_ids; - for (const auto& [object_id, state] : removed_object_states_) { + 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) { - removed_object_ids.push_back(object_id); + // 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 << ")"; @@ -206,10 +211,10 @@ std::vector ActiveWindowChangeDetector::updateRemovedFilter( } MLOG(2) << "[ActiveWindowChangeDetector] Removed-object filter: " - << removed_object_ids.size() << " removed out of " + << removed_objects.size() << " removed out of " << removed_object_states_.size() << " tracked candidates"; - return removed_object_ids; + return removed_objects; } GlobalIndex ActiveWindowChangeDetector::to2DIndex(const Point& point, float voxel_size_inv) { @@ -427,7 +432,7 @@ void ActiveWindowChangeDetector::call(const FrameData& data, 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_object_ids = updateRemovedFilter(free_ratio_measurements); + 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). @@ -436,12 +441,12 @@ void ActiveWindowChangeDetector::call(const FrameData& data, // 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_object_ids.size() + 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_object_ids, newly_added_tracks, current_T_prior_); + sinks_, prior_graph_, removed_objects, newly_added_tracks, current_T_prior_); } void ActiveWindowChangeDetector::loadPriorMap() { 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 c425f4a..3840d73 100644 --- a/khronos_ros/CMakeLists.txt +++ b/khronos_ros/CMakeLists.txt @@ -33,6 +33,7 @@ add_library( 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 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 index 2f1835e..5311e65 100644 --- a/khronos_ros/include/khronos_ros/active_window/tf_icp_publisher.h +++ b/khronos_ros/include/khronos_ros/active_window/tf_icp_publisher.h @@ -80,7 +80,7 @@ class TfIcpPublisher : public ActiveWindowChangeDetector::ActiveWindowCDSink { virtual ~TfIcpPublisher() = default; void call(const DynamicSceneGraph::Ptr& dsg, - const std::vector& removed_object_ids, + const std::vector& removed_objects, const std::vector& newly_added_tracks, const Eigen::Isometry3d& current_T_prior) const override; 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 index 183b533..475542f 100644 --- 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 @@ -95,15 +95,16 @@ class ActiveWindowChangeDetectorVisualizer : public ActiveWindowChangeDetector:: // KhronosSink callback - called each frame. void call(const DynamicSceneGraph::Ptr& dsg, - const std::vector& removed_object_ids, + 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_object_ids) 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; 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 index b2fe781..dd44885 100644 --- a/khronos_ros/src/active_window/tf_icp_publisher.cpp +++ b/khronos_ros/src/active_window/tf_icp_publisher.cpp @@ -76,7 +76,7 @@ TfIcpPublisher::TfIcpPublisher(const Config& cfg, const ianvs::NodeHandle* nh) } void TfIcpPublisher::call(const DynamicSceneGraph::Ptr& /*dsg*/, - const std::vector& /*removed_object_ids*/, + 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). diff --git a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp index 4cfa8f4..e01bfaf 100644 --- a/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp +++ b/khronos_ros/src/visualization/active_window_change_detector_visualizer.cpp @@ -91,7 +91,7 @@ ActiveWindowChangeDetectorVisualizer::ActiveWindowChangeDetectorVisualizer( void ActiveWindowChangeDetectorVisualizer::call( const DynamicSceneGraph::Ptr& dsg, - const std::vector& removed_object_ids, + 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. @@ -107,8 +107,8 @@ void ActiveWindowChangeDetectorVisualizer::call( // print out removed object ids MLOG(3) << "[ActiveWindowChangeDetectorVisualizer] Object "; - for (const auto& id : removed_object_ids) { - MLOG(3) << spark_dsg::NodeSymbol(id).str() << ", "; + for (const auto& obj : removed_objects) { + MLOG(3) << spark_dsg::NodeSymbol(obj.id).str() << ", "; } MLOG(3) << " are removed"; // set stamps for all visualizations @@ -116,7 +116,7 @@ void ActiveWindowChangeDetectorVisualizer::call( stamp_is_set_ = true; // Visualize removed objects - visualizeChangedObjects(dsg, removed_object_ids); + visualizeChangedObjects(dsg, removed_objects); visualizeAddedObjects(newly_added_tracks, current_T_prior); stamp_is_set_ = false; } @@ -148,21 +148,20 @@ void ActiveWindowChangeDetectorVisualizer::drawPriorGraph(const DynamicSceneGrap void ActiveWindowChangeDetectorVisualizer::visualizeChangedObjects( const DynamicSceneGraph::Ptr& dsg, - const std::vector& removed_object_ids) const { + 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_object_ids.size()); + new_markers.markers.reserve(removed_objects.size()); std_msgs::msg::Header header; header.frame_id = config.global_frame_name; header.stamp = getStamp(); - for (size_t i = 0u; i < removed_object_ids.size(); ++i) { - const auto& id = removed_object_ids[i]; - const auto& object_node = dsg->getNode(id); + 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; @@ -173,7 +172,7 @@ void ActiveWindowChangeDetectorVisualizer::visualizeChangedObjects( // 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(id & 0xffffffff); + marker.id = static_cast(obj.id & 0xffffffff); } MarkerArray msg; From 7743c829fcb8a61aa2f7763b64ccd2a7e8543310 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 8 Jul 2026 14:20:46 -0400 Subject: [PATCH 20/27] initial version for track saving. --- khronos/CMakeLists.txt | 3 +- .../khronos/active_window/track_saver_sink.h | 142 +++++++ .../src/active_window/track_saver_sink.cpp | 345 ++++++++++++++++++ 3 files changed, 489 insertions(+), 1 deletion(-) create mode 100644 khronos/include/khronos/active_window/track_saver_sink.h create mode 100644 khronos/src/active_window/track_saver_sink.cpp diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index 8dd0f4d..20711ec 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -15,7 +15,7 @@ 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) @@ -53,6 +53,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 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..cf7f633 --- /dev/null +++ b/khronos/include/khronos/active_window/track_saver_sink.h @@ -0,0 +1,142 @@ +/** ----------------------------------------------------------------------------- + * 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/active_window.h" + +namespace khronos { + +/** + * @brief Active-window KhronosSink that dumps raw per-observation data for every track to disk, + * one folder per track (`track_/`), for offline debugging of tracker fragmentation. Runs + * incrementally: each frame, any track observed at the current stamp gets its latest observation + * appended to its folder. Each data product (masks, color/depth images, poses, camera intrinsics, + * colored point clouds, track metadata) can be toggled independently in the config. + */ +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 the binary instance mask for the track's observation this frame. + bool save_masks = true; + + //! Save the color image and a red-mask overlay visualization for this frame. + bool save_color = true; + + //! Save the depth image (16-bit PNG in mm + raw float32 binary) for this frame. + bool save_depth = true; + + //! Save the sensor pose in world frame (translation, rotation, 4x4 transform) for this frame. + bool save_pose = true; + + //! Save the camera intrinsics once per track (only for pinhole hydra::Camera sensors). + 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_meta.json summary (semantics, confidence, bbox, observation count). + bool save_metadata = true; + } const config; + + // Construction. + explicit ActiveWindowTrackSaver(const Config& config); + virtual ~ActiveWindowTrackSaver() = default; + + /** + * @brief For every track observed in this frame, append its latest observation's data to its + * track_/ folder under the run's timestamped output directory. + * @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 saveColor(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + const cv::Mat* binary_mask) const; + void saveDepth(const std::string& track_dir, const std::string& ts_str, const FrameData& data) const; + void savePose(const std::string& track_dir, const std::string& ts_str, const FrameData& data) const; + void saveCameraIntrinsics(const std::string& track_dir, const FrameData& data, int track_id) const; + void savePointcloud(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data, + const cv::Mat* binary_mask) const; + void saveMetadata(const std::string& track_dir, const Track& track) const; + + //! Cached base output directory for this run, computed lazily on first call(). + mutable std::string base_dir_; + + //! Track IDs whose camera_intrinsics.json has already been written. + mutable std::set intrinsics_saved_; +}; + +void declare_config(ActiveWindowTrackSaver::Config& config); + +} // namespace khronos 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..c84d518 --- /dev/null +++ b/khronos/src/active_window/track_saver_sink.cpp @@ -0,0 +1,345 @@ +/** ----------------------------------------------------------------------------- + * 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 "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_masks, "save_masks"); + field(config.save_color, "save_color"); + field(config.save_depth, "save_depth"); + field(config.save_pose, "save_pose"); + field(config.save_camera_intrinsics, "save_camera_intrinsics"); + field(config.save_pointcloud, "save_pointcloud"); + field(config.pointcloud_frame, "pointcloud_frame"); + field(config.save_metadata, "save_metadata"); + 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() + "/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::saveColor(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()) { + return; + } + cv::Mat bgr_image; + cv::cvtColor(data.input.color_image, bgr_image, cv::COLOR_RGB2BGR); + cv::imwrite(track_dir + "/color_" + ts_str + ".png", bgr_image); + + if (binary_mask && !binary_mask->empty()) { + 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::saveDepth(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data) const { + if (data.input.depth_image.empty()) { + return; + } + // 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(track_dir + "/depth_" + ts_str + ".png", depth_mm); + + // Raw float32 binary for exact values. + std::ofstream depth_stream(track_dir + "/depth_" + ts_str + ".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()); + } +} + +void ActiveWindowTrackSaver::savePose(const std::string& track_dir, + const std::string& ts_str, + const FrameData& data) const { + std::ofstream pose_stream(track_dir + "/pose_" + ts_str + ".txt"); + if (!pose_stream.is_open()) { + return; + } + 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 ActiveWindowTrackSaver::saveCameraIntrinsics(const std::string& track_dir, + const FrameData& data, + int track_id) const { + if (intrinsics_saved_.count(track_id)) { + return; + } + const auto* camera = dynamic_cast(&data.input.getSensor()); + if (!camera) { + return; + } + const auto& cam_config = camera->getConfig(); + std::ofstream camera_stream(track_dir + "/camera_intrinsics.json"); + if (!camera_stream.is_open()) { + return; + } + camera_stream << "{\n" + << " \"fx\": " << cam_config.fx << ",\n" + << " \"fy\": " << cam_config.fy << ",\n" + << " \"cx\": " << cam_config.cx << ",\n" + << " \"cy\": " << cam_config.cy << ",\n" + << " \"width\": " << cam_config.width << ",\n" + << " \"height\": " << cam_config.height << "\n" + << "}\n"; + intrinsics_saved_.insert(track_id); +} + +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::saveMetadata(const std::string& track_dir, const Track& track) const { + std::ofstream meta_stream(track_dir + "/track_meta.json"); + if (!meta_stream.is_open()) { + return; + } + meta_stream << std::fixed << std::setprecision(6); + meta_stream << "{\n" + << " \"id\": " << track.id << ",\n" + << " \"semantic_category_id\": " + << (track.semantics ? std::to_string(track.semantics->category_id) : "null") << ",\n" + << " \"confidence\": " << track.confidence << ",\n" + << " \"is_dynamic\": " << (track.is_dynamic ? "true" : "false") << ",\n" + << " \"is_active\": " << (track.is_active ? "true" : "false") << ",\n" + << " \"first_seen_ns\": " << track.first_seen << ",\n" + << " \"last_seen_ns\": " << track.last_seen << ",\n" + << " \"num_observations\": " << track.observations.size() << ",\n" + << " \"pointcloud_frame\": \"" << config.pointcloud_frame << "\",\n" + << " \"bounding_box\": {\n" + << " \"center\": [" << track.last_bounding_box.world_P_center.x() << ", " + << track.last_bounding_box.world_P_center.y() << ", " + << track.last_bounding_box.world_P_center.z() << "],\n" + << " \"dimensions\": [" << track.last_bounding_box.dimensions.x() << ", " + << track.last_bounding_box.dimensions.y() << ", " + << track.last_bounding_box.dimensions.z() << "]\n" + << " }\n" + << "}\n"; +} + +void ActiveWindowTrackSaver::call(const FrameData& data, + const VolumetricMap& /*map*/, + const Tracks& tracks) const { + 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); + const std::string ts_str = std::to_string(obs.stamp); + + cv::Mat binary_mask; + if (config.save_masks) { + saveMask(track_dir, ts_str, data, obs.semantic_cluster_id, &binary_mask); + } + if (config.save_color) { + saveColor(track_dir, ts_str, data, binary_mask.empty() ? nullptr : &binary_mask); + } + if (config.save_depth) { + saveDepth(track_dir, ts_str, data); + } + if (config.save_pose) { + savePose(track_dir, ts_str, data); + } + if (config.save_camera_intrinsics) { + saveCameraIntrinsics(track_dir, data, track.id); + } + if (config.save_pointcloud) { + savePointcloud(track_dir, ts_str, data, binary_mask.empty() ? nullptr : &binary_mask); + } + if (config.save_metadata) { + saveMetadata(track_dir, track); + } + } +} + +} // namespace khronos From 033ddf759cf5a97d012992311099eb907d0ecb89 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 8 Jul 2026 16:30:39 -0400 Subject: [PATCH 21/27] add serialization method for Track class. --- khronos/CMakeLists.txt | 9 ++ khronos/app/test_track_serialization.cpp | 152 ++++++++++++++++++ .../khronos/active_window/data/track.h | 16 ++ .../khronos/active_window/track_saver_sink.h | 9 +- khronos/src/active_window/data/track.cpp | 142 ++++++++++++++++ .../src/active_window/track_saver_sink.cpp | 37 +---- 6 files changed, 332 insertions(+), 33 deletions(-) create mode 100644 khronos/app/test_track_serialization.cpp diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index 20711ec..eca2161 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -98,6 +98,15 @@ ${PROJECT_NAME} 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}) + ########## # Export # ########## 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/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/track_saver_sink.h b/khronos/include/khronos/active_window/track_saver_sink.h index cf7f633..e08ca82 100644 --- a/khronos/include/khronos/active_window/track_saver_sink.h +++ b/khronos/include/khronos/active_window/track_saver_sink.h @@ -52,7 +52,7 @@ namespace khronos { * one folder per track (`track_/`), for offline debugging of tracker fragmentation. Runs * incrementally: each frame, any track observed at the current stamp gets its latest observation * appended to its folder. Each data product (masks, color/depth images, poses, camera intrinsics, - * colored point clouds, track metadata) can be toggled independently in the config. + * colored point clouds, serialized Track state) can be toggled independently in the config. */ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { public: @@ -88,8 +88,9 @@ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { //! coincident tracks show up near the origin), or "sensor" (relative to the sensor optical pose). std::string pointcloud_frame = "world"; - //! Save/overwrite a track_meta.json summary (semantics, confidence, bbox, observation count). - bool save_metadata = true; + //! 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; } const config; // Construction. @@ -128,7 +129,7 @@ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { const std::string& ts_str, const FrameData& data, const cv::Mat* binary_mask) const; - void saveMetadata(const std::string& track_dir, const Track& track) const; + void saveTrackJson(const std::string& track_dir, const Track& track) const; //! Cached base output directory for this run, computed lazily on first call(). mutable std::string base_dir_; diff --git a/khronos/src/active_window/data/track.cpp b/khronos/src/active_window/data/track.cpp index 60f65d8..48ee554 100644 --- a/khronos/src/active_window/data/track.cpp +++ b/khronos/src/active_window/data/track.cpp @@ -37,7 +37,149 @@ #include "khronos/active_window/data/track.h" +#include + +#include +#include // Eigen adl_serializer (Vector3f, VectorXf, ...) + 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 SemanticClusterInfo& semantics) { + json j{{"category_id", semantics.category_id}}; + // Only persist the feature vector when it actually carries openset info; a size-1 feature is + // the "no features" placeholder (see SemanticClusterInfo default) and not worth the size. + if (semantics.feature.size() > 1) { + j["feature"] = semantics.feature; + } + return j; +} + +SemanticClusterInfo semanticsFromJson(const json& j) { + SemanticClusterInfo semantics(j.at("category_id").get()); + if (j.contains("feature")) { + semantics.feature = j.at("feature").get(); + } + return semantics; +} + +// BoundingBox is serialized 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 here. +json toJson(const BoundingBox& bbox) { + return 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 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; +} + +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", 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 = 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 = 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/track_saver_sink.cpp b/khronos/src/active_window/track_saver_sink.cpp index c84d518..f8f6d0b 100644 --- a/khronos/src/active_window/track_saver_sink.cpp +++ b/khronos/src/active_window/track_saver_sink.cpp @@ -71,7 +71,7 @@ void declare_config(ActiveWindowTrackSaver::Config& config) { field(config.save_camera_intrinsics, "save_camera_intrinsics"); field(config.save_pointcloud, "save_pointcloud"); field(config.pointcloud_frame, "pointcloud_frame"); - field(config.save_metadata, "save_metadata"); + field(config.save_track_json, "save_track_json"); 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", @@ -276,32 +276,11 @@ void ActiveWindowTrackSaver::savePointcloud(const std::string& track_dir, } } -void ActiveWindowTrackSaver::saveMetadata(const std::string& track_dir, const Track& track) const { - std::ofstream meta_stream(track_dir + "/track_meta.json"); - if (!meta_stream.is_open()) { - return; - } - meta_stream << std::fixed << std::setprecision(6); - meta_stream << "{\n" - << " \"id\": " << track.id << ",\n" - << " \"semantic_category_id\": " - << (track.semantics ? std::to_string(track.semantics->category_id) : "null") << ",\n" - << " \"confidence\": " << track.confidence << ",\n" - << " \"is_dynamic\": " << (track.is_dynamic ? "true" : "false") << ",\n" - << " \"is_active\": " << (track.is_active ? "true" : "false") << ",\n" - << " \"first_seen_ns\": " << track.first_seen << ",\n" - << " \"last_seen_ns\": " << track.last_seen << ",\n" - << " \"num_observations\": " << track.observations.size() << ",\n" - << " \"pointcloud_frame\": \"" << config.pointcloud_frame << "\",\n" - << " \"bounding_box\": {\n" - << " \"center\": [" << track.last_bounding_box.world_P_center.x() << ", " - << track.last_bounding_box.world_P_center.y() << ", " - << track.last_bounding_box.world_P_center.z() << "],\n" - << " \"dimensions\": [" << track.last_bounding_box.dimensions.x() << ", " - << track.last_bounding_box.dimensions.y() << ", " - << track.last_bounding_box.dimensions.z() << "]\n" - << " }\n" - << "}\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::call(const FrameData& data, @@ -336,8 +315,8 @@ void ActiveWindowTrackSaver::call(const FrameData& data, if (config.save_pointcloud) { savePointcloud(track_dir, ts_str, data, binary_mask.empty() ? nullptr : &binary_mask); } - if (config.save_metadata) { - saveMetadata(track_dir, track); + if (config.save_track_json) { + saveTrackJson(track_dir, track); } } } From 7cca6951bf21d2096660cc1f96082afc69e99c50 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 8 Jul 2026 17:55:56 -0400 Subject: [PATCH 22/27] step 3 complete, we now have a new formalized tracking and framedata saving mechanism. --- khronos/CMakeLists.txt | 15 + khronos/app/test_frame_data_serialization.cpp | 221 ++++++++++ khronos/app/test_track_association.cpp | 409 ++++++++++++++++++ .../khronos/active_window/data/frame_data.h | 46 ++ .../khronos/active_window/track_saver_sink.h | 56 +-- khronos/include/khronos/utils/json_utils.h | 62 +++ khronos/src/active_window/data/frame_data.cpp | 345 +++++++++++++++ khronos/src/active_window/data/track.cpp | 44 +- .../src/active_window/track_saver_sink.cpp | 160 +++---- khronos/src/utils/json_utils.cpp | 76 ++++ 10 files changed, 1263 insertions(+), 171 deletions(-) create mode 100644 khronos/app/test_frame_data_serialization.cpp create mode 100644 khronos/app/test_track_association.cpp create mode 100644 khronos/include/khronos/utils/json_utils.h create mode 100644 khronos/src/active_window/data/frame_data.cpp create mode 100644 khronos/src/utils/json_utils.cpp diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index eca2161..c2ca2bb 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -20,6 +20,7 @@ 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) @@ -44,6 +45,7 @@ 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 @@ -71,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 ) @@ -107,6 +110,18 @@ 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}) + ########## # Export # ########## 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..d440f4b --- /dev/null +++ b/khronos/app/test_track_association.cpp @@ -0,0 +1,409 @@ +/** ----------------------------------------------------------------------------- + * 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 \ + * --existing-track-stamp \ + * --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. + * - --existing-track-stamp is important: without it, the existing track's last_points/ + * last_bounding_box come from its FINAL saved snapshot (Track only ever persists one, see + * Track::save), which may be far in time/viewpoint from the frames under test and will show + * artificially low IoU. Pass the existing track's own observation stamp immediately + * preceding the gap you're investigating (found via + * `cat /tracks/track_0/track.json | grep stamp`) to test the tracker's real + * historical decision point instead. + * - --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. + */ + +#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; +} + +// 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; +} + +} // 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.5f; + float min_cosine_sim = 0.0f; + float min_cross_iou = 0.5f; + int min_num_observations = 20; + std::string bbox_type = "aabb"; + long long existing_track_stamp = -1; + + 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. " + "Use this to test the tracker's real historical decision point rather than an " + "artifact of Track only persisting its final state."); + + 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) { + 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); + + const size_t tracks_before = tracks.size(); + const size_t seed_obs_before = has_seed_track ? tracks[0].observations.size() : 0; + + 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 if (has_seed_track) { + const size_t seed_obs_after = tracks[0].observations.size(); + if (seed_obs_after > seed_obs_before) { + std::cout << "RESULT: ASSOCIATED into seed track (observations " << seed_obs_before + << " -> " << seed_obs_after << ")\n"; + } else { + std::cout << "RESULT: no track count change and no observation added -- detection was " + "dropped silently.\n"; + } + } else { + const size_t total_obs_after = + std::accumulate(tracks.begin(), tracks.end(), size_t{0}, + [](size_t sum, const Track& t) { return sum + t.observations.size(); }); + std::cout << "RESULT: associated into one of the " << tracks.size() + << " existing track(s) (total observations now " << total_obs_after << ")\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/include/khronos/active_window/data/frame_data.h b/khronos/include/khronos/active_window/data/frame_data.h index e051806..1df04fc 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,48 @@ 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. + * @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); }; +/** + * @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/track_saver_sink.h b/khronos/include/khronos/active_window/track_saver_sink.h index e08ca82..30ad661 100644 --- a/khronos/include/khronos/active_window/track_saver_sink.h +++ b/khronos/include/khronos/active_window/track_saver_sink.h @@ -37,7 +37,6 @@ #pragma once -#include #include #include @@ -48,11 +47,14 @@ namespace khronos { /** - * @brief Active-window KhronosSink that dumps raw per-observation data for every track to disk, - * one folder per track (`track_/`), for offline debugging of tracker fragmentation. Runs - * incrementally: each frame, any track observed at the current stamp gets its latest observation - * appended to its folder. Each data product (masks, color/depth images, poses, camera intrinsics, - * colored point clouds, serialized Track state) can be toggled independently in the config. + * @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: @@ -65,19 +67,15 @@ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { //! created underneath it on first use to avoid collisions between runs. std::string output_directory; - //! Save the binary instance mask for the track's observation this frame. - bool save_masks = true; - - //! Save the color image and a red-mask overlay visualization for this frame. - bool save_color = true; - - //! Save the depth image (16-bit PNG in mm + raw float32 binary) for this frame. - bool save_depth = true; + //! 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 sensor pose in world frame (translation, rotation, 4x4 transform) for this frame. - bool save_pose = true; + //! Save the binary instance mask crop for the track's observation this frame. + bool save_masks = true; - //! Save the camera intrinsics once per track (only for pinhole hydra::Camera sensors). + //! 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. @@ -91,6 +89,10 @@ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { //! 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. @@ -98,8 +100,8 @@ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { virtual ~ActiveWindowTrackSaver() = default; /** - * @brief For every track observed in this frame, append its latest observation's data to its - * track_/ folder under the run's timestamped output directory. + * @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. @@ -118,24 +120,22 @@ class ActiveWindowTrackSaver : public ActiveWindow::KhronosSink { const FrameData& data, int semantic_cluster_id, cv::Mat* binary_mask_out) const; - void saveColor(const std::string& track_dir, - const std::string& ts_str, - const FrameData& data, - const cv::Mat* binary_mask) const; - void saveDepth(const std::string& track_dir, const std::string& ts_str, const FrameData& data) const; - void savePose(const std::string& track_dir, const std::string& ts_str, const FrameData& data) const; - void saveCameraIntrinsics(const std::string& track_dir, const FrameData& data, int track_id) 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_; - //! Track IDs whose camera_intrinsics.json has already been written. - mutable std::set intrinsics_saved_; + //! Whether camera_intrinsics.json has already been written for this run. + mutable bool intrinsics_saved_ = false; }; void declare_config(ActiveWindowTrackSaver::Config& config); 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/src/active_window/data/frame_data.cpp b/khronos/src/active_window/data/frame_data.cpp new file mode 100644 index 0000000..5e95c55 --- /dev/null +++ b/khronos/src/active_window/data/frame_data.cpp @@ -0,0 +1,345 @@ +/** ----------------------------------------------------------------------------- + * 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 "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(); +} + +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) { + 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) { + pixels_by_id[id].emplace_back(u, v); + } + } + } + return pixels_by_id; +} + +void loadClustersJson(const std::string& dir, int width, int height, 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 object_pixels = gatherPixelsById(object_image); + const auto dynamic_pixels = gatherPixelsById(dynamic_image); + + 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) { + 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). + + 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, 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 48ee554..d78b44c 100644 --- a/khronos/src/active_window/data/track.cpp +++ b/khronos/src/active_window/data/track.cpp @@ -42,6 +42,8 @@ #include #include // Eigen adl_serializer (Vector3f, VectorXf, ...) +#include "khronos/utils/json_utils.h" + namespace khronos { namespace { @@ -61,42 +63,6 @@ Observation observationFromJson(const json& j) { return obs; } -json toJson(const SemanticClusterInfo& semantics) { - json j{{"category_id", semantics.category_id}}; - // Only persist the feature vector when it actually carries openset info; a size-1 feature is - // the "no features" placeholder (see SemanticClusterInfo default) and not worth the size. - if (semantics.feature.size() > 1) { - j["feature"] = semantics.feature; - } - return j; -} - -SemanticClusterInfo semanticsFromJson(const json& j) { - SemanticClusterInfo semantics(j.at("category_id").get()); - if (j.contains("feature")) { - semantics.feature = j.at("feature").get(); - } - return semantics; -} - -// BoundingBox is serialized 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 here. -json toJson(const BoundingBox& bbox) { - return 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 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; -} - json toJson(const GlobalIndexSet& voxels) { json arr = json::array(); for (const GlobalIndex& voxel : voxels) { @@ -117,7 +83,7 @@ json toJson(const Track& track) { json j{{"id", track.id}, {"last_seen", track.last_seen}, {"first_seen", track.first_seen}, - {"last_bounding_box", toJson(track.last_bounding_box)}, + {"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}, @@ -140,7 +106,7 @@ Track trackFromJson(const json& j) { 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 = boundingBoxFromJson(j.at("last_bounding_box")); + 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(); @@ -154,7 +120,7 @@ Track trackFromJson(const json& j) { } if (!j.at("semantics").is_null()) { - track.semantics = semanticsFromJson(j.at("semantics")); + track.semantics = khronos::semanticsFromJson(j.at("semantics")); } return track; } diff --git a/khronos/src/active_window/track_saver_sink.cpp b/khronos/src/active_window/track_saver_sink.cpp index f8f6d0b..5bbf347 100644 --- a/khronos/src/active_window/track_saver_sink.cpp +++ b/khronos/src/active_window/track_saver_sink.cpp @@ -39,6 +39,7 @@ #include #include +#include #include #include #include @@ -47,6 +48,7 @@ #include #include +#include "khronos/active_window/data/frame_data.h" #include "khronos/utils/output_file_utils.h" namespace khronos { @@ -64,14 +66,13 @@ void declare_config(ActiveWindowTrackSaver::Config& 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_color, "save_color"); - field(config.save_depth, "save_depth"); - field(config.save_pose, "save_pose"); 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", @@ -97,7 +98,7 @@ std::string ActiveWindowTrackSaver::getBaseDir() const { } std::string ActiveWindowTrackSaver::getTrackDir(int track_id) const { - const std::string track_dir = getBaseDir() + "/track_" + std::to_string(track_id); + const std::string track_dir = getBaseDir() + "/tracks/track_" + std::to_string(track_id); ensureDirectoryExists(track_dir); return track_dir; } @@ -117,98 +118,21 @@ void ActiveWindowTrackSaver::saveMask(const std::string& track_dir, } } -void ActiveWindowTrackSaver::saveColor(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()) { +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::imwrite(track_dir + "/color_" + ts_str + ".png", bgr_image); - if (binary_mask && !binary_mask->empty()) { - 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::saveDepth(const std::string& track_dir, - const std::string& ts_str, - const FrameData& data) const { - if (data.input.depth_image.empty()) { - return; - } - // 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(track_dir + "/depth_" + ts_str + ".png", depth_mm); - - // Raw float32 binary for exact values. - std::ofstream depth_stream(track_dir + "/depth_" + ts_str + ".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()); - } -} - -void ActiveWindowTrackSaver::savePose(const std::string& track_dir, - const std::string& ts_str, - const FrameData& data) const { - std::ofstream pose_stream(track_dir + "/pose_" + ts_str + ".txt"); - if (!pose_stream.is_open()) { - return; - } - 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 ActiveWindowTrackSaver::saveCameraIntrinsics(const std::string& track_dir, - const FrameData& data, - int track_id) const { - if (intrinsics_saved_.count(track_id)) { - return; - } - const auto* camera = dynamic_cast(&data.input.getSensor()); - if (!camera) { - return; - } - const auto& cam_config = camera->getConfig(); - std::ofstream camera_stream(track_dir + "/camera_intrinsics.json"); - if (!camera_stream.is_open()) { - return; - } - camera_stream << "{\n" - << " \"fx\": " << cam_config.fx << ",\n" - << " \"fy\": " << cam_config.fy << ",\n" - << " \"cx\": " << cam_config.cx << ",\n" - << " \"cy\": " << cam_config.cy << ",\n" - << " \"width\": " << cam_config.width << ",\n" - << " \"height\": " << cam_config.height << "\n" - << "}\n"; - intrinsics_saved_.insert(track_id); + 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, @@ -283,9 +207,46 @@ void ActiveWindowTrackSaver::saveTrackJson(const std::string& track_dir, const T 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. @@ -294,23 +255,11 @@ void ActiveWindowTrackSaver::call(const FrameData& data, } const Observation& obs = track.observations.back(); const std::string track_dir = getTrackDir(track.id); - const std::string ts_str = std::to_string(obs.stamp); cv::Mat binary_mask; if (config.save_masks) { saveMask(track_dir, ts_str, data, obs.semantic_cluster_id, &binary_mask); - } - if (config.save_color) { - saveColor(track_dir, ts_str, data, binary_mask.empty() ? nullptr : &binary_mask); - } - if (config.save_depth) { - saveDepth(track_dir, ts_str, data); - } - if (config.save_pose) { - savePose(track_dir, ts_str, data); - } - if (config.save_camera_intrinsics) { - saveCameraIntrinsics(track_dir, data, track.id); + 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); @@ -318,6 +267,9 @@ void ActiveWindowTrackSaver::call(const FrameData& data, if (config.save_track_json) { saveTrackJson(track_dir, track); } + if (config.save_track_symlinks) { + createTrackSymlinks(track_dir, ts_str); + } } } 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 From a26840a19994b1d080fd1503b4d796f56b6c922e Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 8 Jul 2026 21:40:59 -0400 Subject: [PATCH 23/27] initial version of full pipeline test --- khronos/CMakeLists.txt | 6 + khronos/app/full_run_tracker_replay.cpp | 392 ++++++++++++++++++++++++ 2 files changed, 398 insertions(+) create mode 100644 khronos/app/full_run_tracker_replay.cpp diff --git a/khronos/CMakeLists.txt b/khronos/CMakeLists.txt index c2ca2bb..5b118c2 100644 --- a/khronos/CMakeLists.txt +++ b/khronos/CMakeLists.txt @@ -122,6 +122,12 @@ add_executable(test_frame_data_serialization app/test_frame_data_serialization.c 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..16de14b --- /dev/null +++ b/khronos/app/full_run_tracker_replay.cpp @@ -0,0 +1,392 @@ +/** ----------------------------------------------------------------------------- + * 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 \ + * --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 + * + * IMPORTANT: --track-by / --min-semantic-iou / etc. default to MaxIoUTracker::Config's in-code + * defaults, NOT necessarily what a given run's config used (e.g. this codebase's default_awcd + * config uses min_semantic_iou: 0.25, not the Config default of 0.5) -- pass the real deployed + * values (base_params/hydra.yaml or an experiment override's active_window.tracker/map_window + * blocks) to reproduce a run faithfully, or deliberately change them to test whether a different + * config would have avoided a known fragmentation case. + * + * --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; + float min_semantic_iou = 0.5f; + float min_cosine_sim = 0.0f; + float min_cross_iou = 0.5f; + int min_num_observations = 20; + 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; + + 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_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("--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("--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); + 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; +} From ef76277342269d7955c6ba694e2d1fef8f2fed98 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 8 Jul 2026 21:43:34 -0400 Subject: [PATCH 24/27] frame data construct rgb too --- khronos/src/active_window/data/frame_data.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/khronos/src/active_window/data/frame_data.cpp b/khronos/src/active_window/data/frame_data.cpp index 5e95c55..ca2f24a 100644 --- a/khronos/src/active_window/data/frame_data.cpp +++ b/khronos/src/active_window/data/frame_data.cpp @@ -188,6 +188,19 @@ bool loadDepth(const std::string& dir, int width, int height, cv::Mat* depth) { 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()) { @@ -284,6 +297,8 @@ FrameData::Ptr FrameData::load(const std::string& observation_dir, 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; From 6d27a78b1d41bf94c2a56c2b77946fa4b7ea254a Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 9 Jul 2026 11:42:53 -0400 Subject: [PATCH 25/27] Getting full tracker replay to work. - Nan depth values are forward to the module producing Nan bounding boxes - Filter out Nan value when loading frame data - Test tracker association now saves the reprojected mask - Instance forwarding now to Nan filtering --- khronos/app/full_run_tracker_replay.cpp | 65 +++++++--- khronos/app/test_track_association.cpp | 119 ++++++++++++++++-- .../khronos/active_window/data/frame_data.h | 11 +- .../active_window/tracking/max_iou_tracker.h | 12 ++ khronos/src/active_window/data/frame_data.cpp | 43 +++++-- .../free_space_motion_detector.cpp | 5 + .../object_detection/instance_forwarding.cpp | 6 + .../tracking/max_iou_tracker.cpp | 15 ++- 8 files changed, 230 insertions(+), 46 deletions(-) diff --git a/khronos/app/full_run_tracker_replay.cpp b/khronos/app/full_run_tracker_replay.cpp index 16de14b..9de608b 100644 --- a/khronos/app/full_run_tracker_replay.cpp +++ b/khronos/app/full_run_tracker_replay.cpp @@ -63,17 +63,26 @@ * 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 + * --track-by pixels --map-window-type spatial --map-window-radius 14.0 \ + * --min-range 0.05 --max-range 10.0 * - * IMPORTANT: --track-by / --min-semantic-iou / etc. default to MaxIoUTracker::Config's in-code - * defaults, NOT necessarily what a given run's config used (e.g. this codebase's default_awcd - * config uses min_semantic_iou: 0.25, not the Config default of 0.5) -- pass the real deployed - * values (base_params/hydra.yaml or an experiment override's active_window.tracker/map_window - * blocks) to reproduce a run faithfully, or deliberately change them to test whether a different - * config would have avoided a known fragmentation case. + * 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. @@ -217,10 +226,15 @@ int main(int argc, char** argv) { std::string run_dir; std::string output_dir; int verbosity = 0; - float min_semantic_iou = 0.5f; + // 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.5f; - int min_num_observations = 20; + 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"; @@ -228,6 +242,8 @@ int main(int argc, char** argv) { 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() @@ -237,15 +253,17 @@ int main(int argc, char** argv) { 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_val(0.5f); + 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_val(0.5f); - app.add_option( - "--min-num-observations", min_num_observations, "MaxIoUTracker::Config::min_num_observations") - ->default_val(20); + 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)") @@ -256,6 +274,15 @@ int main(int argc, char** argv) { 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); @@ -310,8 +337,8 @@ int main(int argc, char** argv) { 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); + 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; diff --git a/khronos/app/test_track_association.cpp b/khronos/app/test_track_association.cpp index d440f4b..c7f4137 100644 --- a/khronos/app/test_track_association.cpp +++ b/khronos/app/test_track_association.cpp @@ -67,20 +67,18 @@ * test_track_association \ * --existing-track-dir /tracks/track_0 \ * --observation-track-dir /tracks/track_3 \ - * --existing-track-stamp \ * --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. - * - --existing-track-stamp is important: without it, the existing track's last_points/ - * last_bounding_box come from its FINAL saved snapshot (Track only ever persists one, see - * Track::save), which may be far in time/viewpoint from the frames under test and will show - * artificially low IoU. Pass the existing track's own observation stamp immediately - * preceding the gap you're investigating (found via - * `cat /tracks/track_0/track.json | grep stamp`) to test the tracker's real - * historical decision point instead. + * - 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 @@ -104,6 +102,11 @@ * 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 candidate track's (tracks[0]) reprojected last_points in blue + * overlaid with the frame's real detection cluster in green (only meaningful for the default + * --track-by pixels; unset by default, so no files are written unless requested). */ #include @@ -113,6 +116,8 @@ #include #include +#include +#include #include "khronos/active_window/data/frame_data.h" #include "khronos/active_window/data/track.h" @@ -162,6 +167,20 @@ std::optional> clusterIdAtStamp(const Track& track, TimeSta 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 @@ -230,6 +249,43 @@ bool overrideWithHistoricalObservation(const std::string& track_dir, TimeStamp s return true; } +// Saves a dual-color comparison image: cluster's real pixels in green, the track's reprojected +// points (see MaxIoUTracker::reprojectPoints) in blue -- drawn second so overlap reads as blue. +// Mirrors ActiveWindowTrackSaver::saveOverlay's blend style (0.6/0.4 addWeighted, applied twice). +void saveReprojectionOverlay(const std::string& dir, + TimeStamp stamp, + const FrameData& frame_data, + const MeasurementCluster& cluster, + const std::set& reprojected_pixels) { + 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 overlay = bgr_image.clone(); + overlay.setTo(cv::Scalar(0, 255, 0), detection_mask); // Green in BGR: real detection. + overlay.setTo(cv::Scalar(255, 0, 0), reprojected_mask); // Blue in BGR: reprojected track. + cv::Mat blended; + cv::addWeighted(bgr_image, 0.6, overlay, 0.4, 0, blended); + cv::imwrite(dir + "/reprojection_" + std::to_string(stamp) + ".png", blended); +} + } // namespace int main(int argc, char** argv) { @@ -242,12 +298,13 @@ int main(int argc, char** argv) { std::string existing_track_dir; std::string observation_track_dir; int verbosity = 6; - float min_semantic_iou = 0.5f; + float min_semantic_iou = 0.25f; float min_cosine_sim = 0.0f; - float min_cross_iou = 0.5f; - int min_num_observations = 20; + 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; @@ -283,8 +340,16 @@ int main(int argc, char** argv) { 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. " - "Use this to test the tracker's real historical decision point rather than an " - "artifact of Track only persisting its final state."); + "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); @@ -301,6 +366,23 @@ int main(int argc, char** argv) { 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), @@ -361,6 +443,17 @@ int main(int argc, char** argv) { // 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); + saveReprojectionOverlay( + save_reprojection_dir, stamp, *frame_data, kept_clusters.front(), reprojected_pixels); + } + } + const size_t tracks_before = tracks.size(); const size_t seed_obs_before = has_seed_track ? tracks[0].observations.size() : 0; diff --git a/khronos/include/khronos/active_window/data/frame_data.h b/khronos/include/khronos/active_window/data/frame_data.h index 1df04fc..3c2414c 100644 --- a/khronos/include/khronos/active_window/data/frame_data.h +++ b/khronos/include/khronos/active_window/data/frame_data.h @@ -102,11 +102,20 @@ struct FrameData { * @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); + TimeStamp stamp, + float min_range = 0.f, + float max_range = 0.f); }; /** 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 17eb464..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 @@ -133,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/src/active_window/data/frame_data.cpp b/khronos/src/active_window/data/frame_data.cpp index ca2f24a..cec1ae4 100644 --- a/khronos/src/active_window/data/frame_data.cpp +++ b/khronos/src/active_window/data/frame_data.cpp @@ -38,6 +38,7 @@ #include "khronos/active_window/data/frame_data.h" #include +#include #include #include #include @@ -211,7 +212,10 @@ bool loadRawInt32(const std::string& path, int width, int height, cv::Mat* out) return static_cast(file) || file.eof(); } -std::unordered_map gatherPixelsById(const cv::Mat& image) { +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; @@ -219,15 +223,34 @@ std::unordered_map gatherPixelsById(const cv::Mat& image) { 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) { - pixels_by_id[id].emplace_back(u, v); + 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, FrameData* frame_data) { +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."; @@ -245,8 +268,9 @@ void loadClustersJson(const std::string& dir, int width, int height, FrameData* frame_data->dynamic_image = dynamic_image; } - const auto object_pixels = gatherPixelsById(object_image); - const auto dynamic_pixels = gatherPixelsById(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(); @@ -282,7 +306,9 @@ void FrameData::save(const std::string& observation_dir) const { FrameData::Ptr FrameData::load(const std::string& observation_dir, const std::shared_ptr& camera, - TimeStamp stamp) { + TimeStamp stamp, + float min_range, + float max_range) { if (!camera) { LOG(ERROR) << "[FrameData] load() requires a valid camera."; return nullptr; @@ -310,7 +336,8 @@ FrameData::Ptr FrameData::load(const std::string& observation_dir, } auto frame_data = std::make_shared(input); - loadClustersJson(observation_dir, cam_config.width, cam_config.height, frame_data.get()); + loadClustersJson( + observation_dir, cam_config.width, cam_config.height, min_range, max_range, frame_data.get()); return frame_data; } 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/tracking/max_iou_tracker.cpp b/khronos/src/active_window/tracking/max_iou_tracker.cpp index e22cd94..46fc2a6 100644 --- a/khronos/src/active_window/tracking/max_iou_tracker.cpp +++ b/khronos/src/active_window/tracking/max_iou_tracker.cpp @@ -557,20 +557,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; From 0238925c209a23826faefca27adc22009ff8c1ef Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 9 Jul 2026 11:56:22 -0400 Subject: [PATCH 26/27] Better color blending. - Red for current frame mask, green for reprojected mask, blended yellow for intersecting pixels. --- khronos/app/test_track_association.cpp | 46 ++++++++++++++++++-------- 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/khronos/app/test_track_association.cpp b/khronos/app/test_track_association.cpp index c7f4137..34cd5e5 100644 --- a/khronos/app/test_track_association.cpp +++ b/khronos/app/test_track_association.cpp @@ -104,15 +104,18 @@ * trusting a Mode 1 result. * * Both modes accept --save-reprojection-dir to additionally save, per replayed frame, - * reprojection_.png -- the candidate track's (tracks[0]) reprojected last_points in blue - * overlaid with the frame's real detection cluster in green (only meaningful for the default - * --track-by pixels; unset by default, so no files are written unless requested). + * 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 @@ -249,14 +252,25 @@ bool overrideWithHistoricalObservation(const std::string& track_dir, TimeStamp s return true; } -// Saves a dual-color comparison image: cluster's real pixels in green, the track's reprojected -// points (see MaxIoUTracker::reprojectPoints) in blue -- drawn second so overlap reads as blue. -// Mirrors ActiveWindowTrackSaver::saveOverlay's blend style (0.6/0.4 addWeighted, applied twice). +// 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) { + const std::set& reprojected_pixels, + float iou) { if (frame_data.input.color_image.empty()) { return; } @@ -278,12 +292,14 @@ void saveReprojectionOverlay(const std::string& dir, } } - cv::Mat overlay = bgr_image.clone(); - overlay.setTo(cv::Scalar(0, 255, 0), detection_mask); // Green in BGR: real detection. - overlay.setTo(cv::Scalar(255, 0, 0), reprojected_mask); // Blue in BGR: reprojected track. - cv::Mat blended; - cv::addWeighted(bgr_image, 0.6, overlay, 0.4, 0, blended); - cv::imwrite(dir + "/reprojection_" + std::to_string(stamp) + ".png", blended); + 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 @@ -449,8 +465,10 @@ int main(int argc, char** argv) { 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); + save_reprojection_dir, stamp, *frame_data, kept_clusters.front(), reprojected_pixels, iou); } } From 17f44f16da711af144e796c664f4650aaab78d53 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 9 Jul 2026 12:04:48 -0400 Subject: [PATCH 27/27] better tracker debug message print. --- khronos/app/test_track_association.cpp | 40 +++++++++++++------ .../tracking/max_iou_tracker.cpp | 10 ++++- 2 files changed, 36 insertions(+), 14 deletions(-) diff --git a/khronos/app/test_track_association.cpp b/khronos/app/test_track_association.cpp index 34cd5e5..b3a344b 100644 --- a/khronos/app/test_track_association.cpp +++ b/khronos/app/test_track_association.cpp @@ -473,7 +473,10 @@ int main(int argc, char** argv) { } const size_t tracks_before = tracks.size(); - const size_t seed_obs_before = has_seed_track ? tracks[0].observations.size() : 0; + 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); @@ -485,21 +488,34 @@ int main(int argc, char** argv) { std::cout << "RESULT: " << (is_initial_creation ? "INITIAL TRACK CREATED" : "NEW TRACK " "CREATED (fragmentation reproduced)") << ". tracks.size() " << tracks_before << " -> " << tracks.size() << "\n"; - } else if (has_seed_track) { - const size_t seed_obs_after = tracks[0].observations.size(); - if (seed_obs_after > seed_obs_before) { - std::cout << "RESULT: ASSOCIATED into seed track (observations " << seed_obs_before - << " -> " << seed_obs_after << ")\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"; } - } else { - const size_t total_obs_after = - std::accumulate(tracks.begin(), tracks.end(), size_t{0}, - [](size_t sum, const Track& t) { return sum + t.observations.size(); }); - std::cout << "RESULT: associated into one of the " << tracks.size() - << " existing track(s) (total observations now " << total_obs_after << ")\n"; } } diff --git a/khronos/src/active_window/tracking/max_iou_tracker.cpp b/khronos/src/active_window/tracking/max_iou_tracker.cpp index 46fc2a6..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()) {