Move util_probability inside p3iv

p3iv_modules/package.xml

@@ -15,10 +15,9 @@
 
   <depend>lanelet2_core</depend>
   <depend>lanelet2_matching</depend>
-  <depend>util_probability</depend>
   <depend>p3iv_utils</depend>
   <depend>p3iv_utils_polyvision</depend>
-
+  <depend>p3iv_utils_probability</depend>
 
   <export>
     <rosdoc config="../rosdoc.yaml" />

p3iv_types/include/p3iv_types/motion_state.hpp

@@ -1,7 +1,7 @@
 #pragma once
-#include "util_probability/bivariate_distribution.hpp"
-#include "util_probability/sequence_distribution.hpp"
-#include "util_probability/univariate_distribution.hpp"
+#include "p3iv_utils_probability/bivariate_distribution.hpp"
+#include "p3iv_utils_probability/sequence_distribution.hpp"
+#include "p3iv_utils_probability/univariate_distribution.hpp"
 
 namespace p3iv_types {
 

p3iv_types/package.xml

@@ -14,7 +14,7 @@
   <test_depend>gtest</test_depend>
 
   <depend>pybind11-dev</depend>
-  <depend>util_probability</depend>
+  <depend>p3iv_utils_probability</depend>
 
   <export>
     <rosdoc config="../rosdoc.yaml" />

p3iv_types/src/p3iv_types/motion_state.py

@@ -1,7 +1,9 @@
-
 import numpy as np
-from util_probability.distributions import UnivariateNormalDistribution, BivariateNormalDistribution
-from util_probability.distributions import UnivariateNormalDistributionSequence, BivariateNormalDistributionSequence
+from p3iv_utils_probability.distributions import UnivariateNormalDistribution, BivariateNormalDistribution
+from p3iv_utils_probability.distributions import (
+    UnivariateNormalDistributionSequence,
+    BivariateNormalDistributionSequence,
+)
 from p3iv_utils.finite_differences import finite_differences
 from p3iv_utils.helper_functions import get_yaw_angle
 

p3iv_utils_probability/.gitignore

@@ -0,0 +1,5 @@
+*~
+setup.py
+*.pyc
+CMakeLists.txt.user
+

p3iv_utils_probability/.gitlab-ci.yml

@@ -0,0 +1,5 @@
+#version=1.5
+include:
+   - project: 'pub/mrt_cmake_modules'
+     ref: master
+     file: '/ci_templates/default_catkin_project.yml'

p3iv_utils_probability/CMakeLists.txt

@@ -0,0 +1,69 @@
+set(MRT_PKG_VERSION 4.0.0)
+# Modify only if you know what you are doing!
+cmake_minimum_required(VERSION 3.5.1)
+project(p3iv_utils_probability)
+
+###################
+## Find packages ##
+###################
+find_package(mrt_cmake_modules REQUIRED)
+include(UseMrtStdCompilerFlags)
+include(GatherDeps)
+
+# You can add a custom.cmake in order to add special handling for this package. E.g. you can do:
+# list(REMOVE_ITEM DEPENDEND_PACKAGES <package name 1> <package name 2> ...)
+# To remove libs which cannot be found automatically. You can also "find_package" other, custom dependencies there.
+# You can also set PROJECT_INSTALL_FILES to install files that are not installed by default.
+if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake")
+    include("${CMAKE_CURRENT_SOURCE_DIR}/custom.cmake")
+endif()
+
+find_package(AutoDeps REQUIRED COMPONENTS ${DEPENDEND_PACKAGES})
+
+mrt_parse_package_xml()
+
+########################
+## Add python modules ##
+########################
+# This adds a python module if located under src/{PROJECT_NAME)
+mrt_python_module_setup()
+
+mrt_glob_files(PROJECT_PYTHON_SOURCE_FILES_SRC "python_api/*.cpp")
+if (PROJECT_PYTHON_SOURCE_FILES_SRC)
+    # Add a cpp-python api library. Make sure there are no name collisions with python modules in this project
+    mrt_add_python_api( ${PROJECT_NAME}
+        FILES ${PROJECT_PYTHON_SOURCE_FILES_SRC}
+        )
+endif()
+
+############################
+## Read source code files ##
+############################
+mrt_glob_files_recurse(PROJECT_HEADER_FILES_INC "include/*.h" "include/*.hpp" "include/*.cuh")
+mrt_glob_files(PROJECT_SOURCE_FILES_INC "src/*.h" "src/*.hpp" "src/*.cuh")
+mrt_glob_files(PROJECT_SOURCE_FILES_SRC "src/*.cpp" "src/*.cu")
+
+###########
+## Build ##
+###########
+# Declare a cpp library
+mrt_add_library(${PROJECT_NAME}
+    INCLUDES ${PROJECT_HEADER_FILES_INC} ${PROJECT_SOURCE_FILES_INC}
+    SOURCES ${PROJECT_SOURCE_FILES_SRC}
+    )
+
+#############
+## Install ##
+#############
+# Install all targets, headers by default and scripts and other files if specified (folders or files).
+# This command also exports libraries and config files for dependent packages and this supersedes catkin_package.
+mrt_install(PROGRAMS scripts FILES res data ${PROJECT_INSTALL_FILES})
+
+#############
+## Testing ##
+#############
+# Add test targets for cpp and python tests
+if (CATKIN_ENABLE_TESTING)
+    mrt_add_tests(test)
+    mrt_add_nosetests(test)
+endif()

p3iv_utils_probability/README.md

@@ -0,0 +1,15 @@
+# UTIL PROBABILITY
+
+Definitions and functions for probability distributions
+
+### [Doxygen documentation](http://tas.private.MRT.pages.mrt.uni-karlsruhe.de/util_probability/doxygen/index.html)
+### [Coverage report](http://tas.private.MRT.pages.mrt.uni-karlsruhe.de/util_probability/coverage/index.html)
+
+## Usage
+
+TODO: Write usage instructions
+
+
+```
+scipy>=1.2.1
+```

p3iv_utils_probability/include/p3iv_utils_probability/bivariate_distribution.hpp

@@ -0,0 +1,39 @@
+#pragma once
+#include "internal/normal.hpp"
+
+namespace util_probability {
+
+
+template <typename T>
+class BivariateNormalDistribution : public NormalDistribution<T, 2> {
+public:
+    BivariateNormalDistribution() = default;
+
+    explicit BivariateNormalDistribution(T meanX, T meanY) {
+        this->_mean << meanX, meanY;
+    }
+
+    BivariateNormalDistribution(T meanX, T meanY, T covXX, T covXY, T covYX, T covYY) {
+        this->_mean << meanX, meanY;
+        this->_covariance << covXX, covXY, covYX, covYY;
+    }
+
+    void setCovariance(T covXX, T covXY, T covYX, T covYY) {
+        this->_covariance << covXX, covXY, covYX, covYY;
+    }
+
+    void setMean(T meanX, T meanY) {
+        this->_mean << meanX, meanY;
+    }
+
+    std::vector<T> range(const double& n) const {
+
+        std::vector<T> rangeValues;
+        rangeValues.resize(5);
+
+        // todo: calculate ellipse parameters
+
+        return rangeValues;
+    }
+};
+} // namespace util_probability

p3iv_utils_probability/include/p3iv_utils_probability/internal/approximations.hpp

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <cassert>
+#include <cmath>
+#include <vector>
+
+
+namespace util_probability {
+
+
+template <class T>
+static inline T abs(const T& a) {
+    return a < T(0.0) ? -a : a;
+}
+
+
+inline double abs(double x) {
+    return std::abs(x);
+}
+
+/// Erf implementation according to Abramowitz & Stegun
+/// A polynomial approximation with a maximal error of 1.2e-7. This approximation is based on Chebyshev fitting.
+/// Numerical Recipes in Fortran 77: The Art of Scientific Computing
+/// (ISBN 0-521-43064-X), 1992, page 214, Cambridge University Press.
+/// Implementation is based on:
+/// source: http://rextester.com/discussion/FVYOC97045/std-erf-versus-erf-impl-Abramowitz-Stegun-
+/// which is almost the same in the source book Fortran77 recipes.
+/// See also: https://en.wikipedia.org/wiki/Error_function#Polynomial
+template <typename T>
+inline T erf_impl(T v) {
+    const T t = T(1) / (T(1) + T(0.5) * abs(v));
+
+    static const T c[] = {T(1.26551223),
+                          T(1.00002368),
+                          T(0.37409196),
+                          T(0.09678418),
+                          T(-0.18628806),
+                          T(0.27886807),
+                          T(-1.13520398),
+                          T(1.48851587),
+                          T(-0.82215223),
+                          T(0.17087277)};
+
+    T result =
+        T(1) -
+        t * std::exp(
+                (-v * v) - c[0] +
+                t * (c[1] +
+                     t * (c[2] +
+                          t * (c[3] + t * (c[4] + t * (c[5] + t * (c[6] + t * (c[7] + t * (c[8] + t * (c[9]))))))))));
+
+    return (v >= T(0)) ? result : -result;
+}
+
+
+template <typename T>
+inline T norm_cdf(const T& v, const T& mean, const T& std) {
+    return 0.5 * (1.0 + erf_impl((v - mean) * (M_SQRT1_2 / std)));
+}
+
+template <typename T>
+inline T trnorm_cdf(T v, const double& mean, const double& std, const double& a, const double& b) {
+    /* a = lower truncation bound
+       b = upper truncation bound
+    */
+    assert(a < b);
+
+    T zeta = T((v - mean) / std);
+    T alpha = T((a - mean) / std);
+    T beta = T((b - mean) / std);
+    T cdf_alpha = T(0); /* if -inf */
+    T cdf_beta = T(1);  /* if +inf, by definition of the cdf */
+
+    if (!std::isinf(a))
+        cdf_alpha = norm_cdf(alpha, 0.0, 1.0);
+
+    if (!std::isinf(b))
+        cdf_beta = norm_cdf(beta, 0.0, 1.0);
+
+
+    return (norm_cdf(zeta, 0.0, 1.0) - cdf_alpha) / (cdf_beta - cdf_alpha);
+}
+
+} // namespace util_probability

p3iv_utils_probability/include/p3iv_utils_probability/internal/normal.hpp

@@ -0,0 +1,68 @@
+#pragma once
+#include <iostream>
+#include <Eigen/Core>
+#include <glog/logging.h>
+
+namespace util_probability {
+
+template <typename T, int Dim>
+struct NormalDistribution {
+
+    using Mean = Eigen::Matrix<T, Dim, 1>;
+    using Covariance = Eigen::Matrix<T, Dim, Dim>;
+
+
+    NormalDistribution() : _mean{Mean::Zero()}, _covariance{Covariance::Zero()} {
+    }
+
+    NormalDistribution(const Eigen::Ref<const Mean>& mean_) : _mean{mean_}, _covariance{Covariance::Zero()} {
+    }
+
+    NormalDistribution(const Eigen::Ref<const Mean>& mean_, const Eigen::Ref<const Covariance>& covariance_)
+            : _mean{mean_}, _covariance{covariance_} {
+        LOG_ASSERT(_covariance.rows() == _covariance.cols());
+        LOG_ASSERT(_mean.size() == _covariance.rows());
+    }
+
+    NormalDistribution(const Eigen::Ref<const Mean>& mean_, Eigen::Matrix<T, Dim, 1>& variance_)
+            : _mean{mean_}, _covariance{Covariance::Zero()} {
+
+        for (size_t i = 0; i < variance_.size(); i++) {
+            _covariance(i, i) = variance_(i);
+        }
+    }
+
+    size_t dimension() const {
+        return static_cast<size_t>(_mean.size());
+    }
+
+    Mean mean() const {
+        return _mean;
+    }
+
+    T mean(size_t r, size_t c) const {
+        return _mean(r, c);
+    }
+
+    Covariance covariance() const {
+        return _covariance;
+    }
+
+    T covariance(size_t r, size_t c) const {
+        return _covariance(r, c);
+    }
+
+    virtual Eigen::Matrix<T, Dim, 1> variance() const {
+        Eigen::Matrix<T, Dim, 1> variance;
+        for (size_t i = 0; i < _covariance.rows(); i++) {
+            variance(i) = _covariance(i, i);
+        }
+        return variance;
+    }
+
+protected:
+    Mean _mean;
+    Covariance _covariance;
+};
+
+} // namespace util_probability