Program Listing for File pose3.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_POSE3_HPP
#define HOLOSCAN_POSE_TREE_MATH_POSE3_HPP

#include <cmath>

#include "holoscan/pose_tree/math/pose2.hpp"
#include "holoscan/pose_tree/math/so3.hpp"
#include "holoscan/pose_tree/math/types.hpp"

namespace holoscan {

template <typename K>
struct Pose3 {
  using Scalar = K;
  static constexpr int kDimension = 3;

  SO3<K> rotation;
  Vector3<K> translation;

  Pose3() : rotation(SO3<K>::identity()), translation(Vector3<K>::Zero()) {}

  Pose3(const SO3<K>& rotation, const Vector3<K>& translation)
      : rotation(rotation), translation(translation) {}

  static Pose3 identity() { return Pose3{SO3<K>::identity(), Vector3<K>::Zero()}; }

  static Pose3 from_translation(const Vector3<K>& translation) {
    return Pose3{SO3<K>::identity(), translation};
  }

  static Pose3 from_translation(K x, K y, K z) {
    return Pose3{SO3<K>::identity(), Vector3<K>{x, y, z}};
  }

  static Pose3 from_rotation(const Vector3<K>& axis, K angle) {
    return Pose3{SO3<K>::from_axis_angle(axis, angle), Vector3<K>::Zero()};
  }

  static Pose3 from_pose2_xy(const Pose2<K>& pose) {
    return Pose3{SO3<K>::from_so2_xy(pose.rotation),
                 Vector3<K>{pose.translation.x(), pose.translation.y(), K(0)}};
  }

  static Pose3 from_matrix(const Matrix4<K>& matrix) {
    return Pose3{
        SO3<K>::from_normalized_quaternion(Quaternion<K>(matrix.template topLeftCorner<3, 3>())),
        matrix.template topRightCorner<3, 1>()};
  }

  Pose3 inverse() const {
    const auto inv = rotation.inverse();
    return Pose3{inv, -(inv * translation)};
  }

  Matrix4<K> matrix() const {
    Matrix4<K> ret;
    ret.template topLeftCorner<3, 3>() = rotation.matrix();
    ret.template topRightCorner<3, 1>() = translation;
    ret.template bottomLeftCorner<1, 3>().setZero();
    ret(3, 3) = K(1);
    return ret;
  }

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

  Pose2<K> to_pose2_xy() const {
    return Pose2<K>{rotation.to_so2_xy(), translation.template head<2>()};
  }

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

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

  Pose3 pow(K exponent) const {
    // First step: align the rotation vector with the z axis.
    const K angle = rotation.angle();
    if (is_almost_zero(angle)) {
      // TODO(bbutin): Use Taylor expansion in that case?
      return Pose3{rotation, exponent * translation};
    }
    const Vector3<K> axis = rotation.axis();
    const Vector3<K> cross = axis.cross(Vector3<K>(K(0), K(0), K(1)));
    const K cos = axis.z();
    const K sin = cross.norm();

    Matrix3<K> rot = Matrix3<K>::Identity() * cos;
    // If axis is align in the Z direction, the matrix above will do the trick, if not we compute
    // rotation matrix to transform the axis in the Z axis.
    if (!is_almost_zero(sin)) {
      // TODO(bbutin): Use Taylor expansion in case sin ~ 0?
      const Vector3<K> unit = cross / sin;
      rot += (1.0 - cos) * unit * unit.transpose();
      rot(0, 1) += -cross.z();
      rot(0, 2) += cross.y();
      rot(1, 2) += -cross.x();
      rot(1, 0) += cross.z();
      rot(2, 0) += -cross.y();
      rot(2, 1) += cross.x();
    }
    // Now we can compute the exponentiation the same way as for the 2d for x and y, while z will
    // just be linearly FromScaledAxis
    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> rot2 = SO2<K>::from_angle(half_angle * (exponent - K(1)));
    const Vector3<K> t = rot * translation;
    const Vector2<K> xy = csc_sin * (rot2 * t.template head<2>());
    const SO3<K> final_rotation = SO3<K>::from_angle_axis(angle * exponent, axis);
    return Pose3{final_rotation, rot.inverse() * Vector3<K>(xy.x(), xy.y(), t.z() * exponent)};
  }
};

using Pose3d = Pose3<double>;
using Pose3f = Pose3<float>;

template <typename K>
Pose3<K> pose3_exp(const Vector6<K>& tangent) {
  return Pose3<K>{SO3<K>::from_scaled_axis(tangent.template tail<3>()), tangent.template head<3>()};
}

template <typename K>
Vector6<K> pose3_log(const Pose3<K>& pose) {
  Vector6<K> result;
  result.template head<3>() = pose.translation;
  result.template tail<3>() = pose.rotation.angle() * pose.rotation.axis();
  return result;
}

template <typename K>
bool is_pose_almost_identity(const Pose3<K>& pose) {
  return is_almost_zero(pose.translation.norm()) && is_almost_zero(pose.rotation.angle());
}

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

}  // namespace holoscan

#endif/* HOLOSCAN_POSE_TREE_MATH_POSE3_HPP */