multivariate_distribution_traits.hpp

Go to the documentation of this file.

// Copyright 2024 Ekumen, Inc.
//
// 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 BELUGA_RANDOM_MULTIVARIATE_DISTRIBUTION_TRAITS_HPP
#define BELUGA_RANDOM_MULTIVARIATE_DISTRIBUTION_TRAITS_HPP
 
#include <Eigen/Core>
#include <sophus/se2.hpp>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
 
namespace beluga {
 
template <class T, class Enable = void>
struct multivariate_distribution_traits;
 
template <class T>
struct multivariate_distribution_traits<T, std::enable_if_t<std::is_base_of_v<Eigen::EigenBase<T>, T>>> {
  static_assert(T::ColsAtCompileTime == 1 || T::RowsAtCompileTime == 1, "T should be a column or row vector");
 
  static constexpr int matrix_size = T::ColsAtCompileTime > T::RowsAtCompileTime  //
                                         ? T::ColsAtCompileTime
                                         : T::RowsAtCompileTime;
 
  using scalar_type = typename T::Scalar;
 
  using result_type = typename T::PlainMatrix;
 
  using vector_type = typename T::PlainMatrix;
 
  using covariance_type = typename Eigen::Matrix<scalar_type, matrix_size, matrix_size>;
 
  [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) { return t; }
 
  [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) { return v; }
};
 
template <class T>
struct multivariate_distribution_traits<T, std::enable_if_t<std::is_base_of_v<Sophus::SO2Base<T>, T>>> {
  using scalar_type = typename T::Scalar;
 
  using result_type = Sophus::SO2<scalar_type>;
 
  using vector_type = typename Eigen::Matrix<scalar_type, 1, 1>;
 
  using covariance_type = typename Eigen::Matrix<scalar_type, 1, 1>;
 
  [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) { return vector_type{t.log()}; }
 
  [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) {
    return Sophus::SO2<scalar_type>::exp(v.x());
  }
};
 
template <class T>
struct multivariate_distribution_traits<T, std::enable_if_t<std::is_base_of_v<Sophus::SE2Base<T>, T>>> {
  using scalar_type = typename T::Scalar;
 
  using result_type = Sophus::SE2<scalar_type>;
 
  using vector_type = typename Eigen::Matrix<scalar_type, 3, 1>;
 
  using covariance_type = typename Eigen::Matrix<scalar_type, 3, 3>;
 
  [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) {
    vector_type v;
    v << t.translation(), t.so2().log();
    return v;
  }
 
  [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) {
    return {Sophus::SO2<scalar_type>::exp(v.z()), v.head(2)};
  }
};
 
template <class T>
struct multivariate_distribution_traits<T, std::enable_if_t<std::is_base_of_v<Sophus::SO3Base<T>, T>>> {
  using scalar_type = typename T::Scalar;
 
  using result_type = Sophus::SO3<scalar_type>;
 
  using vector_type = typename Eigen::Matrix<scalar_type, 3, 1>;
 
  using covariance_type = typename Eigen::Matrix<scalar_type, 3, 3>;
 
  [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) { return t.log(); }
 
  [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) {
    // Projecting a Gaussian through the exponential map is not exactly correct.
    // Rotations in Lie groups don't have infinite support, making the term 'normal' distribution incorrect.
    // This approach follows the SO2/SE2 method, facing similar issues in 3D.
    // For small covariances, the resulting rotations in SO3 approximate a normal distribution.
    return Sophus::SO3<scalar_type>::exp(v);
  }
};
 
template <class T>
struct multivariate_distribution_traits<T, std::enable_if_t<std::is_base_of_v<Sophus::SE3Base<T>, T>>> {
  using scalar_type = typename T::Scalar;
 
  using result_type = Sophus::SE3<scalar_type>;
 
  using vector_type = typename Eigen::Matrix<scalar_type, 6, 1>;
 
  using covariance_type = typename Eigen::Matrix<scalar_type, 6, 6>;
 
  [[nodiscard]] static constexpr vector_type to_vector(const result_type& t) {
    vector_type v;
    v << t.translation(), t.so3().log();
    return v;
  }
 
  [[nodiscard]] static constexpr result_type from_vector(const vector_type& v) {
    // Projecting a Gaussian through the exponential map is not exactly correct.
    // Rotations in Lie groups don't have infinite support, making the term 'normal' distribution incorrect.
    // This approach follows the SO2/SE2 method, facing similar issues in 3D.
    // For small covariances, the resulting rotations in SO3 approximate a normal distribution.
    return result_type{Sophus::SO3<scalar_type>::exp(v.tail(3)), v.head(3)};
  }
};
 
}  // namespace beluga
 
#endif