Merge pull request #16 from USC-ACTLab/wwu/ALP-89-implement-probabili…

…ty-utilities

Wwu/alp 89 implement probability utilities

CMakeLists.txt

@@ -1,6 +1,6 @@
 # project language setup
 cmake_minimum_required(VERSION 3.14)
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # generate compile_commands.json

include/core-structs.h

@@ -41,4 +41,4 @@ struct Observation2D {
     *    Populating/clearing this field may be more trouble than it's worth...
     */
    std::optional<int> landmarkID;
-};
+};

include/math-util.h

@@ -0,0 +1,66 @@
+/**
+ * @file math-util.h
+ * @brief Defines common probability distributions and their samplers.
+ */
+
+#pragma once
+
+#include <vector>
+#include <utility>
+#include <random>
+#include <cmath>
+#include <limits.h>
+#include "core-structs.h"
+
+/**
+ * @brief Namespace for common math utility functions.
+ */
+namespace MathUtil {
+
+
+/**
+ * @brief Transforms a 2d quantity from world frame to body frame
+ * @details Multiplies the 2d vector with rotational matrix given the heading in radians.
+ * World Frame Coordinate: front x, right y, clockwise rotation from x to y as positive
+ *
+ * @param[in]    aWorld_quant  2D quantity in world frame
+ * @param[in]    aTheta_rad    Robot heading angle in randians
+ *
+ * @returns quantity in robot frame
+ */
+std::pair<float, float> worldToBody2D( const std::pair<float, float>& aWorld_quant,
+                                       const float aTheta_rad);
+
+/**
+ * @brief Transforms a 2d quantity from robot frame to world frame
+ * @details Multiplies the 2d vector with inverse rotational matrix given the heading in radians
+ * Body Frame coordinate: front x, right y, clockwise rotation from x to y as positive
+ *
+ * @param[in]    aBody_pose   2D quantity in robot frame
+ * @param[in]    aTheta_rad    Robot heading angle in randians
+ *
+ * @returns quantity in world frame
+ */
+std::pair<float, float> bodyToWorld2D(const std::pair<float, float>& aBody_quant,
+                                      const float aTheta_rad);
+
+/**
+ * @brief Generates a random sample from the Gaussian: N(mean, variance).
+ *
+ * @details Reference: Probabilistic Robotics, Ch. 5.3, Table 5.4 (pg. 124).
+ *    Further reading from Winkler (1995; pg. 293) may be advised.
+ *    C++ has a built in normal distribution template, see:
+ *    https://en.cppreference.com/w/cpp/numeric/random/normal_distribution
+ *
+ *    TODO: Ensure we're talking about stddev and variance correctly,
+ *       because ProbRob has a typo somewhere in Table 5.4. Beware!
+ *
+ * @param[in]     aMean       Mean of the sampled Normal distribution
+ * @param[in]     aVariance   Variance of the sampled Normal distribution
+ *
+ * @returns Random value, distributed according to the defined Gaussian
+ */
+float sampleNormal( const float aMean, const float aVariance );
+
+
+}; // namespace MathUtil

src/FastSLAM/CMakeLists.txt

@@ -1,10 +1,22 @@
-add_library(FastSLAM
-   probability.cpp
+add_library(FastSLAMLib
+   math-util.cpp
 )
 
-target_include_directories(FastSLAM PUBLIC
+target_include_directories(FastSLAMLib PUBLIC
                            "${PROJECT_BINARY_DIR}"
                            "${PROJECT_SOURCE_DIR}/include"
 )
 
-target_link_libraries(FastSLAM Eigen3::Eigen)
+target_link_libraries(FastSLAMLib Eigen3::Eigen)
+
+if(BUILD_TESTS)
+  add_executable(test_MathUtil math-util_test.cpp)
+  target_link_libraries(test_MathUtil
+                        PRIVATE Catch2::Catch2WithMain
+                        FastSLAMLib)
+  catch_discover_tests(test_MathUtil)
+  target_include_directories(test_MathUtil PUBLIC
+    "${PROJECT_BINARY_DIR}"
+    "${PROJECT_SOURCE_DIR}/include"
+  )
+endif()

src/FastSLAM/math-util.cpp

@@ -0,0 +1,41 @@
+/**
+ * @file probability.cpp
+ * @brief Implements common probability distributions and their samplers.
+ */
+
+#include "math-util.h"
+
+std::pair<float, float> MathUtil::worldToBody2D( const std::pair<float,
+                                                                 float>& aWorld_quant,
+                                                 const float aTheta_rad ){
+
+   float x_body = cosf(aTheta_rad) * aWorld_quant.first + sinf(aTheta_rad) *
+                  aWorld_quant.second;
+   float y_body = -sinf(aTheta_rad) * aWorld_quant.first + cosf(aTheta_rad) *
+                  aWorld_quant.second;
+   return std::make_pair(x_body, y_body);
+
+}
+
+std::pair<float, float> MathUtil::bodyToWorld2D( const std::pair<float,
+                                                                 float>& aBody_quant,
+                                                 const float aTheta_rad ){
+
+   float x_world = cosf(aTheta_rad) * aBody_quant.first - sinf(aTheta_rad) *
+                   aBody_quant.second;
+   float y_world = sinf(aTheta_rad) * aBody_quant.first + cosf(aTheta_rad) *
+                   aBody_quant.second;
+   return std::make_pair(x_world, y_world);
+
+}
+
+float MathUtil::sampleNormal( const float aMean, const float aVariance ){
+
+   if (aVariance < 0.0) return std::numeric_limits<double>::quiet_NaN();
+
+   std::random_device rd{};
+   std::mt19937 gen{rd()};
+   std::normal_distribution d{aMean, sqrtf(aVariance)};
+
+   return d(gen);
+}

src/FastSLAM/math-util_test.cpp

@@ -0,0 +1,114 @@
+#include <catch2/catch_test_macros.hpp>
+#include <catch2/matchers/catch_matchers_floating_point.hpp>
+#include "math-util.h"
+#include <math.h>
+
+TEST_CASE("Test Gaussian Sampling - negative variance") {
+   REQUIRE( isnan( MathUtil::sampleNormal(1.0f, -1.0f) ) );
+}
+
+TEST_CASE("Test Gaussian Sampling - zero variance") {
+   REQUIRE(MathUtil::sampleNormal(1.0f, 0.0f) == 1.0f);
+}
+
+TEST_CASE("Test world to body 2D - zero degree heading") {
+   std::pair<float, float> world_frame_speed{1.0f, 1.0f};
+   float heading_rad = 0.0f;
+   auto target = std::make_pair(1.0f, 1.0f);
+
+   auto res = MathUtil::worldToBody2D(world_frame_speed, heading_rad);
+
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second,0.00001f) );
+}
+
+TEST_CASE("Test body to world 2D - zero degree heading") {
+   std::pair<float, float> body_frame_speed{1.0f, 1.0f};
+   float heading_rad = 0.0f;
+   auto target = std::make_pair(1.0f, 1.0f);
+
+   auto res = MathUtil::bodyToWorld2D(body_frame_speed, heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+}
+
+TEST_CASE("Test world to body 2D - 90 degree heading") {
+   std::pair<float, float> world_frame_vel{1.0f, 0.0f};
+   float heading_rad = M_PI / 2;
+   auto target = std::make_pair(0.0f, -1.0f);
+
+   auto res = MathUtil::worldToBody2D(world_frame_vel, heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+}
+
+TEST_CASE("Test body to world 2D - 90 degree heading") {
+   std::pair<float, float> body_frame_vel{1.0f, 0.0f};
+   float heading_rad = M_PI / 2;
+   auto target = std::make_pair(0.0f, 1.0f);
+
+   auto res = MathUtil::bodyToWorld2D(body_frame_vel, heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+}
+
+TEST_CASE("Test world to body 2D - 180 degree heading") {
+   std::pair<float, float> world_frame_vel{1.0f, 0.0f};
+   float heading_rad = M_PI;
+   auto target = std::make_pair(-1.0f, 0.0f);
+
+   auto res = MathUtil::worldToBody2D(world_frame_vel, heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+}
+
+TEST_CASE("Test body to world 2D - 180 degree heading") {
+   std::pair<float, float> world_frame_vel{1.0f, 0.0f};
+   float heading_rad = M_PI;
+   auto target = std::make_pair(-1.0f, 0.0f);
+
+   auto res = MathUtil::worldToBody2D(world_frame_vel, heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+}
+
+TEST_CASE("Test foward-inverse transform: worldToBody") {
+   std::pair<float, float> world_frame_vel{1.0f, 0.0f};
+   float heading_rad = M_PI / 4;
+   auto target = world_frame_vel;
+
+   auto res =
+      MathUtil::bodyToWorld2D(MathUtil::worldToBody2D(world_frame_vel,
+                                                      heading_rad), heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+}
+
+TEST_CASE("Test foward-inverse transform: bodyToWorld") {
+   std::pair<float, float> world_frame_vel{1.0f, 0.0f};
+   float heading_rad = M_PI / 4;
+   auto target = world_frame_vel;
+
+   auto res =
+      MathUtil::worldToBody2D(MathUtil::bodyToWorld2D(world_frame_vel,
+                                                      heading_rad), heading_rad);
+   REQUIRE_THAT( res.first, Catch::Matchers::WithinRel(target.first, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.first, 0.00001f) );
+   REQUIRE_THAT( res.second, Catch::Matchers::WithinRel(target.second, 0.001f) ||
+                            Catch::Matchers::WithinAbs(target.second, 0.00001f) );
+
+}