math_utils.h

Go to the documentation of this file.

/*
Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich.
All rights reserved.
This file is released under the "BSD-3-Clause License".
Please see the LICENSE file that has been included as part of this package.
 */
 
#ifndef HOLISTIC_FUSION_MATH_UTILS_H
#define HOLISTIC_FUSION_MATH_UTILS_H
 
#include <cmath>
#include "holistic_fusion/geometry/Angle.h"
 
namespace holistic_fusion {

inline double rad2deg(double radians) {
  return radians * 180.0 / M_PI;
}

inline float rad2deg(float radians) {
  return (float)(radians * 180.0 / M_PI);
}

inline double deg2rad(double degrees) {
  return degrees * M_PI / 180.0;
}

inline float deg2rad(float degrees) {
  return (float)(degrees * M_PI / 180.0);
}

template <typename PointT>
inline float calcSquaredDiff(const PointT& a, const PointT& b) {
  float diffX = a.x - b.x;
  float diffY = a.y - b.y;
  float diffZ = a.z - b.z;
 
  return diffX * diffX + diffY * diffY + diffZ * diffZ;
}

template <typename PointT>
inline float calcSquaredDiff(const PointT& a, const PointT& b, const float& wb) {
  float diffX = a.x - b.x * wb;
  float diffY = a.y - b.y * wb;
  float diffZ = a.z - b.z * wb;
 
  return diffX * diffX + diffY * diffY + diffZ * diffZ;
}

template <typename PointT>
inline float calcPointDistance(const PointT& p) {
  return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
}

template <typename PointT>
inline float calcSquaredPointDistance(const PointT& p) {
  return p.x * p.x + p.y * p.y + p.z * p.z;
}

template <typename PointT>
inline void rotX(PointT& p, const Angle& ang) {
  float y = p.y;
  p.y = ang.cos() * y - ang.sin() * p.z;
  p.z = ang.sin() * y + ang.cos() * p.z;
}

template <typename PointT>
inline void rotY(PointT& p, const Angle& ang) {
  float x = p.x;
  p.x = ang.cos() * x + ang.sin() * p.z;
  p.z = ang.cos() * p.z - ang.sin() * x;
}

template <typename PointT>
inline void rotZ(PointT& p, const Angle& ang) {
  float x = p.x;
  p.x = ang.cos() * x - ang.sin() * p.y;
  p.y = ang.sin() * x + ang.cos() * p.y;
}

template <typename PointT>
inline void rotateZXY(PointT& p, const Angle& angZ, const Angle& angX, const Angle& angY) {
  rotZ(p, angZ);
  rotX(p, angX);
  rotY(p, angY);
}

template <typename PointT>
inline void rotateYXZ(PointT& p, const Angle& angY, const Angle& angX, const Angle& angZ) {
  rotY(p, angY);
  rotX(p, angX);
  rotZ(p, angZ);
}

inline void invertHomogenousMatrix(const Eigen::Matrix4d& m_in, Eigen::Matrix4d& m_out) {
  m_out = Eigen::Matrix4d::Identity();
  Eigen::Matrix3d R = m_in.block<3, 3>(0, 0);
  Eigen::Vector3d t = m_in.block<3, 1>(0, 3);
  m_out.block<3, 3>(0, 0) = R.transpose();
  m_out.block<3, 1>(0, 3) = -R.transpose() * t;
}
 
}  // namespace holistic_fusion
 
#endif  // HOLISTIC_FUSION_MATH_UTILS_H