Program Listing for File pose2.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_MATH_POSE2_HPP
#define HOLOSCAN_POSE_TREE_MATH_POSE2_HPP

#include <cmath>

#include "holoscan/pose_tree/math/so2.hpp"
#include "holoscan/pose_tree/math/types.hpp"

namespace holoscan {

template <typename K>
struct Pose2 {
  using Scalar = K;
  static constexpr int kDimension = 2;

  SO2<K> rotation;
  Vector2<K> translation;

  static Pose2 identity() { return Pose2{SO2<K>::identity(), Vector2<K>::Zero()}; }

  static Pose2 from_translation(const Vector2<K>& translation) {
    return Pose2{SO2<K>::identity(), translation};
  }

  static Pose2 from_translation(K x, K y) { return Pose2{SO2<K>::identity(), Vector2<K>{x, y}}; }

  static Pose2 from_rotation(const K angle) {
    return Pose2{SO2<K>::from_angle(angle), Vector2<K>::Zero()};
  }

  static Pose2 from_xy_a(K px, K py, K angle) {
    return Pose2{SO2<K>::from_angle(angle), Vector2<K>{px, py}};
  }

  static Pose2 from_matrix(const Matrix3<K>& matrix) {
    return Pose2{SO2<K>::from_normalized(matrix(0, 0), matrix(1, 0)),
                 Vector2<K>(matrix(0, 2), matrix(1, 2))};
  }

  Pose2 inverse() const {
    const SO2<K> inv = rotation.inverse();
    return Pose2{inv, -(inv * translation)};
  }

  Matrix3<K> matrix() const {
    Matrix3<K> ret;
    ret << rotation.cos(), -rotation.sin(), translation.x(), rotation.sin(), rotation.cos(),
        translation.y(), K(0), K(0), K(1);
    return ret;
  }

  template <typename S, typename std::enable_if_t<!std::is_same<S, K>::value, int> = 0>
  Pose2<S> cast() const {
    return Pose2<S>{rotation.template cast<S>(), translation.template cast<S>()};
  }

  template <typename S, typename std::enable_if_t<std::is_same<S, K>::value, int> = 0>
  const Pose2& cast() const {
    // Nothing to do as the type does not change
    return *this;
  }

  friend Pose2 operator*(const Pose2& lhs, const Pose2& rhs) {
    return Pose2{lhs.rotation * rhs.rotation, lhs.rotation * rhs.translation + lhs.translation};
  }

  friend Vector2<K> operator*(const Pose2& pose, const Vector2<K>& vec) {
    return pose.rotation * vec + pose.translation;
  }

  Pose2 pow(K exponent) const {
    const K angle = rotation.angle();
    const K half_angle = angle / K(2);
    const K csc_sin =
        is_almost_zero(angle)
            ? exponent * (K(1) + (K(1) - exponent * exponent) * half_angle * half_angle / K(6))
            : std::sin(half_angle * exponent) / std::sin(half_angle);
    const SO2<K> rot = SO2<K>::from_angle(half_angle * (exponent - K(1)));
    return Pose2{SO2<K>::from_angle(angle * exponent), csc_sin * (rot * translation)};
  }
};

using Pose2d = Pose2<double>;
using Pose2f = Pose2<float>;

template <typename K>
Pose2<K> pose2_exp(const Vector3<K>& tangent) {
  return Pose2<K>{SO2<K>::from_angle(tangent[2]), tangent.template head<2>()};
}

template <typename K>
Vector3<K> pose2_log(const Pose2<K>& pose) {
  return Vector3<K>{pose.translation.x(), pose.translation.y(), pose.rotation.angle()};
}

template <typename K>
bool is_pose_almost_identity(const Pose2<K>& pose) {
  return is_almost_zero(pose.translation.x()) && is_almost_zero(pose.translation.y()) &&
         is_almost_one(pose.rotation.cos());
}

template <typename K>
Vector2<K> pose2_magnitude(const Pose2<K>& pose) {
  return {pose.translation.norm(), std::abs(pose.rotation.angle())};
}

}  // namespace holoscan

#endif/* HOLOSCAN_POSE_TREE_MATH_POSE2_HPP */