Program Listing for File pose_tree.hpp
↰ Return to documentation for file (include/holoscan/pose_tree/pose_tree.hpp)
/*
* SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
* SPDX-License-Identifier: Apache-2.0
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef HOLOSCAN_POSE_TREE_POSE_TREE_HPP
#define HOLOSCAN_POSE_TREE_POSE_TREE_HPP
#include <map>
#include <memory>
#include <mutex>
#include <shared_mutex> // NOLINT(build/include_order)
#include <unordered_map>
#include <utility>
#include <vector>
#include "common/fixed_vector.hpp"
#include "common/unique_index_map.hpp"
#include "holoscan/core/expected.hpp"
#include "holoscan/core/resources/gxf/first_fit_allocator.hpp"
#include "holoscan/logger/logger.hpp"
#include "holoscan/pose_tree/hash_map.hpp"
#include "holoscan/pose_tree/math/pose2.hpp"
#include "holoscan/pose_tree/math/pose3.hpp"
#include "holoscan/pose_tree/pose_tree_history.hpp"
namespace std {
template <class A, class B>
struct hash<pair<A, B>> {
size_t operator()(const pair<A, B>& p) const {
return std::hash<A>{}(p.first) ^ (std::hash<B>{}(p.second) << 1);
}
};
} // namespace std
namespace holoscan {
class PoseTree {
public:
enum class Error {
kInvalidArgument = 0,
kOutOfMemory = 1,
kFrameNotFound = 2,
kAlreadyExists = 3,
kCyclingDependency = 4,
kFramesNotLinked = 5,
kPoseOutOfOrder = 6,
kLogicError = 7,
};
static constexpr int32_t kFrameNameMaximumLength = 127;
static constexpr char const* kAutoGeneratedFrameNamePrefix = "_frame_";
template <typename T>
using expected_t = nvidia::Expected<T, Error>;
using unexpected_t = nvidia::Unexpected<Error>;
using frame_t = uint64_t;
using version_t = uint64_t;
using history_t = uint64_t;
using uid_t = uint64_t;
using CreateFrameCallback = std::function<void(frame_t frame)>;
using SetEdgeCallback =
std::function<void(frame_t lhs, frame_t rhs, double time, const Pose3d& lhs_T_rhs)>;
expected_t<void> init(int32_t number_frames = 1024, int32_t number_edges = 16384,
int32_t history_length = 1048576, int32_t default_number_edges = 16,
int32_t default_history_length = 1024, int32_t edges_chunk_size = 4,
int32_t history_chunk_size = 64);
void deinit();
version_t get_pose_tree_version() const;
expected_t<frame_t> create_frame(std::string_view name, int32_t number_edges);
expected_t<frame_t> create_frame(std::string_view name);
expected_t<frame_t> create_frame(int32_t number_edges);
expected_t<frame_t> create_frame();
expected_t<frame_t> find_frame(std::string_view name) const;
expected_t<frame_t> find_or_create_frame(std::string_view name);
expected_t<frame_t> find_or_create_frame(std::string_view name, int32_t number_edges);
expected_t<version_t> create_edges(frame_t lhs, frame_t rhs);
expected_t<version_t> create_edges(frame_t lhs, frame_t rhs, int32_t maximum_length);
expected_t<version_t> create_edges(frame_t lhs, frame_t rhs,
PoseTreeEdgeHistory::AccessMethod method);
expected_t<version_t> create_edges(frame_t lhs, frame_t rhs, int32_t maximum_length,
PoseTreeEdgeHistory::AccessMethod method);
expected_t<version_t> create_edges(std::string_view lhs, std::string_view rhs);
expected_t<version_t> create_edges(std::string_view lhs, std::string_view rhs,
int32_t maximum_length);
expected_t<version_t> create_edges(std::string_view lhs, std::string_view rhs,
PoseTreeEdgeHistory::AccessMethod method);
expected_t<version_t> create_edges(std::string_view lhs, std::string_view rhs,
int32_t maximum_length,
PoseTreeEdgeHistory::AccessMethod method);
expected_t<version_t> delete_frame(frame_t uid);
expected_t<version_t> delete_frame(std::string_view name);
expected_t<version_t> delete_edge(frame_t lhs, frame_t rhs);
expected_t<version_t> delete_edge(std::string_view lhs, std::string_view rhs);
expected_t<version_t> disconnect_frame(frame_t uid, double time);
expected_t<version_t> disconnect_frame(std::string_view name, double time);
expected_t<version_t> disconnect_edge(frame_t lhs, frame_t rhs, double time);
expected_t<version_t> disconnect_edge(std::string_view lhs, std::string_view rhs, double time);
// Disable all the implicit cast (to make sure to catch a call with the wrong type for the time)
template <class... Args>
expected_t<Pose3d> disconnect_frame(Args&&... args) = delete;
template <class... Args>
expected_t<Pose3d> disconnect_edge(Args&&... args) = delete;
expected_t<std::string_view> get_frame_name(frame_t uid) const;
expected_t<std::pair<Pose3d, double>> get_latest(frame_t lhs, frame_t rhs) const;
expected_t<std::pair<Pose3d, double>> get_latest(std::string_view lhs,
std::string_view rhs) const;
expected_t<Pose3d> get(frame_t lhs, frame_t rhs, double time,
PoseTreeEdgeHistory::AccessMethod method, version_t version) const;
expected_t<Pose3d> get(frame_t lhs, frame_t rhs, double time, version_t version) const;
expected_t<Pose3d> get(frame_t lhs, frame_t rhs, double time,
PoseTreeEdgeHistory::AccessMethod method) const;
expected_t<Pose3d> get(frame_t lhs, frame_t rhs, double time) const;
expected_t<Pose3d> get(frame_t lhs, frame_t rhs, version_t version) const;
expected_t<Pose3d> get(frame_t lhs, frame_t rhs) const;
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs, double time,
PoseTreeEdgeHistory::AccessMethod method, version_t version) const;
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs, double time,
version_t version) const;
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs, version_t version) const;
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs, double time,
PoseTreeEdgeHistory::AccessMethod method) const;
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs, double time) const;
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs) const;
// Disable all the implicit cast (to make sure to catch a call with the wrong type for the time)
template <class... Args>
expected_t<Pose3d> get(frame_t lhs, frame_t rhs, Args&&... args) const = delete;
template <class... Args>
expected_t<Pose3d> get(std::string_view lhs, std::string_view rhs, Args&&... args) const = delete;
template <class... Args>
expected_t<Pose2d> get_pose2_xy(Args&&... args) const {
return get(std::forward<Args>(args)...).map([](const Pose3d& pose_3d) {
return pose_3d.to_pose2_xy();
});
}
expected_t<version_t> set(frame_t lhs, frame_t rhs, double time, const Pose3d& lhs_T_rhs);
expected_t<version_t> set(std::string_view lhs, std::string_view rhs, double time,
const Pose3d& lhs_T_rhs);
expected_t<version_t> set(frame_t lhs, frame_t rhs, double time, const Pose2d& lhs_T_rhs) {
return set(lhs, rhs, time, Pose3d::from_pose2_xy(lhs_T_rhs));
}
expected_t<version_t> set(std::string_view lhs, std::string_view rhs, double time,
const Pose2d& lhs_T_rhs) {
return set(lhs, rhs, time, Pose3d::from_pose2_xy(lhs_T_rhs));
}
// Then we disable all the calls not made with double.
template <typename T, typename Pose>
expected_t<version_t> set(frame_t lhs, frame_t rhs, T time, const Pose& lhs_T_rhs) = delete;
template <typename T, typename Pose>
expected_t<version_t> set(std::string_view lhs, std::string_view rhs, T time,
const Pose& lhs_T_rhs) = delete;
template <typename T>
expected_t<void> get_edge_uids(T& container) const {
std::shared_lock<std::shared_timed_mutex> lock(mutex_);
container.clear();
if (container.capacity() < edges_map_.size()) { return unexpected_t(Error::kOutOfMemory); }
for (const auto& key : edges_map_keys_) {
if (!key) { return unexpected_t(Error::kLogicError); }
const auto value = edges_map_.get(key.value());
if (!value) { return unexpected_t(Error::kLogicError); }
const auto history = histories_map_.try_get(value.value());
if (!history) { return unexpected_t(Error::kLogicError); }
if (!history.value()->connected()) { continue; }
container.push_back(key.value());
}
return expected_t<void>{};
}
template <typename T>
expected_t<void> get_edge_names(T& container) const {
std::shared_lock<std::shared_timed_mutex> lock(mutex_);
container.clear();
if (container.capacity() < edges_map_.size()) { return unexpected_t(Error::kOutOfMemory); }
for (const auto& key : edges_map_keys_) {
if (!key) { return unexpected_t(Error::kLogicError); }
const auto value = edges_map_.get(key.value());
if (!value) { return unexpected_t(Error::kLogicError); }
const auto history = histories_map_.try_get(value.value());
if (!history) { return unexpected_t(Error::kLogicError); }
if (!history.value()->connected()) { continue; }
auto lhs_name = get_frame_name(key.value().first);
auto rhs_name = get_frame_name(key.value().second);
if (!lhs_name || !rhs_name) { continue; }
container.push_back({lhs_name.value(), rhs_name.value()});
}
return expected_t<void>{};
}
template <typename T>
expected_t<void> get_frame_uids(T& container) const {
std::shared_lock<std::shared_timed_mutex> lock(mutex_);
container.clear();
if (container.capacity() < name_to_uid_map_keys_.size()) {
return unexpected_t(Error::kOutOfMemory);
}
for (const auto& key : name_to_uid_map_keys_) {
if (!key) { return unexpected_t(Error::kLogicError); }
const auto value = name_to_uid_map_.get(key.value());
if (!value) { return unexpected_t(Error::kLogicError); }
container.push_back(value.value());
}
return expected_t<void>{};
}
template <typename T>
expected_t<void> get_frame_names(T& container) const {
std::shared_lock<std::shared_timed_mutex> lock(mutex_);
container.clear();
if (container.capacity() < name_to_uid_map_keys_.size()) {
return unexpected_t(Error::kOutOfMemory);
}
for (const auto& key : name_to_uid_map_keys_) {
if (!key) { return unexpected_t(Error::kLogicError); }
container.push_back(key.value());
}
return expected_t<void>{};
}
expected_t<uid_t> add_create_frame_callback(CreateFrameCallback callback);
expected_t<void> remove_create_frame_callback(uid_t cid);
expected_t<uid_t> add_set_edge_callback(SetEdgeCallback callback);
expected_t<void> remove_set_edge_callback(uid_t cid);
static const char* error_to_str(Error error);
private:
struct FrameInfo {
history_t* history;
int32_t number_edges;
int32_t maximum_number_edges;
char name[kFrameNameMaximumLength + 1];
std::string_view name_view;
int32_t distance_to_root;
frame_t node_to_root;
frame_t root;
mutable version_t hint_version;
mutable frame_t dfs_link;
frame_t uid;
};
// Implementation of find_or_create_frame
expected_t<frame_t> find_or_create_frame_impl(std::string_view name, int32_t number_edges);
// Implementation of find_frame
expected_t<frame_t> find_frame_impl(std::string_view name) const;
// Implementation of create_frame
expected_t<frame_t> create_frame_impl(std::string_view name, int32_t number_edges);
// Implementation of create_edges
expected_t<version_t> create_edges_impl(frame_t lhs, frame_t rhs, int32_t maximum_length,
PoseTreeEdgeHistory::AccessMethod method);
// Implementation of delete_edge
expected_t<version_t> delete_edge_impl(frame_t lhs, frame_t rhs, version_t version);
// Update the path to the root for a given connected component starting from the given node.
expected_t<void> update_root(frame_t root);
// Implementation of get using the pre-computed path to the root as a hint. If it fails, it falls
// back to get_dfs_impl.
expected_t<Pose3d> get_impl(frame_t lhs, frame_t rhs, double time,
PoseTreeEdgeHistory::AccessMethod method, version_t version) const;
// Implementation of get that do a dfs to see if a path exists at a given time.
expected_t<Pose3d> get_dfs_impl(frame_t lhs, frame_t rhs, double time,
PoseTreeEdgeHistory::AccessMethod method,
version_t version) const;
mutable std::shared_timed_mutex mutex_;
mutable std::mutex dfs_mutex_;
mutable std::shared_timed_mutex create_frame_callbacks_mutex_;
nvidia::UniqueIndexMap<CreateFrameCallback> create_frame_callbacks_;
nvidia::FixedVector<uid_t> create_frame_callbacks_keys_;
mutable std::shared_timed_mutex set_edge_callbacks_mutex_;
nvidia::UniqueIndexMap<SetEdgeCallback> set_edge_callbacks_;
nvidia::FixedVector<uid_t> set_edge_callbacks_keys_;
// TODO(ben): We need to get rid of std::map
pose_tree::HashMap<std::pair<frame_t, frame_t>, history_t> edges_map_;
nvidia::FixedVector<std::pair<frame_t, frame_t>> edges_map_keys_;
pose_tree::HashMap<std::string_view, frame_t> name_to_uid_map_;
nvidia::FixedVector<std::string_view> name_to_uid_map_keys_;
nvidia::UniqueIndexMap<FrameInfo> frame_map_;
std::unique_ptr<frame_t[]> frames_stack_;
// TODO(ben): We need to get rid of std::map
nvidia::UniqueIndexMap<PoseTreeEdgeHistory> histories_map_;
FirstFitAllocator<history_t> histories_management_;
FirstFitAllocator<PoseTreeEdgeHistory::TimedPose> poses_management_;
version_t version_{};
mutable version_t hint_version_{};
int32_t default_number_edges_{};
int32_t default_history_length_{};
uid_t frame_cb_latest_uid_{};
uid_t edge_cb_latest_uid_{};
};
} // namespace holoscan
#endif/* HOLOSCAN_POSE_TREE_POSE_TREE_HPP */