Program Listing for File pose_tree_history.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_HISTORY_HPP
#define HOLOSCAN_POSE_TREE_POSE_TREE_HISTORY_HPP

#include <shared_mutex>

#include "holoscan/core/expected.hpp"
#include "holoscan/pose_tree/math/pose3.hpp"

namespace holoscan {

class PoseTreeEdgeHistory {
 public:
  enum class AccessMethod {
    kNearest,
    kInterpolateLinearly,
    kExtrapolateLinearly,
    kInterpolateSlerp,
    kExtrapolateSlerp,
    kPrevious,
    kDefault,
  };

  enum class Error {
    kInvalidArgument,
    kOutOfOrder,
    kFramesNotLinked,
    kOutOfRange,
  };

  template <typename T>
  using expected_t = expected<T, Error>;
  using unexpected_t = unexpected<Error>;

  using frame_t = uint64_t;
  using version_t = uint64_t;

  struct TimedPose {
    Pose3d pose;
    double time;
    version_t version;
    bool valid;
  };

  PoseTreeEdgeHistory() = default;

  PoseTreeEdgeHistory(frame_t lhs, frame_t rhs, int32_t maximum_size, TimedPose* buffer);

  PoseTreeEdgeHistory(frame_t lhs, frame_t rhs, int32_t maximum_size, AccessMethod access_method,
                      TimedPose* buffer);

  expected_t<void> set(double time, const Pose3d& pose, version_t version);

  expected_t<TimedPose> at(int32_t index) const;

  expected_t<Pose3d> get(double time, AccessMethod method, version_t version) const;

  expected_t<void> disconnect(double time, version_t version);

  bool connected() const;

  void reset();

  expected_t<TimedPose> latest() const;

  const TimedPose* data() const { return edges_info_; }

  int32_t size() const { return size_; }

  int32_t maximum_size() const { return maximum_size_; }

  frame_t lhs() const { return lhs_; }

  frame_t rhs() const { return rhs_; }

 private:
  expected_t<int32_t> reserve_new_pose(double time, version_t version);

  mutable std::shared_timed_mutex mutex_;
  TimedPose* edges_info_ = nullptr;
  frame_t lhs_, rhs_;
  int32_t maximum_size_ = 0;
  int32_t size_ = 0;
  int32_t pos_ = 0;
  AccessMethod default_access_method_;
};

}  // namespace holoscan

#endif/* HOLOSCAN_POSE_TREE_POSE_TREE_HISTORY_HPP */