From 5bf61fbe8d7751c8a9e6a31c8a72ebf9e4bebb51 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 7 Jan 2022 09:46:02 +0100
Subject: [PATCH] preventing division by zero
---
.../navigation/global_planning/optimization/math.h | 14 ++++++++++++--
1 file changed, 12 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/global_planning/optimization/math.h b/source/armarx/navigation/global_planning/optimization/math.h
index 86f19571..2007edb3 100644
--- a/source/armarx/navigation/global_planning/optimization/math.h
+++ b/source/armarx/navigation/global_planning/optimization/math.h
@@ -22,6 +22,7 @@
#pragma once
#include <array>
+
#include <ceres/ceres.h>
namespace armarx::navigation::global_planning::optimization
@@ -46,8 +47,7 @@ namespace armarx::navigation::global_planning::optimization
return angleDiff;
}
- std::array<double, 2>
- inline periodicDiffJacobian(const auto a, const auto b)
+ std::array<double, 2> inline periodicDiffJacobian(const auto a, const auto b)
{
const auto derivative = [&a, &b]() -> double
{
@@ -56,6 +56,16 @@ namespace armarx::navigation::global_planning::optimization
return 0.;
}
+
+ const double denomSquared =
+ 1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b), 2);
+
+ // prevent division by 0
+ if(denomSquared < 0.001)
+ {
+ return 0.;
+ }
+
return (ceres::sin(a) * ceres::cos(b) - ceres::cos(a) * ceres::sin(b)) /
(ceres::sqrt(
1 - ceres::pow(ceres::sin(a) * ceres::sin(b) + ceres::cos(a) * ceres::cos(b),
--
GitLab