diff --git a/SimoxUtility/math/vector_similarity/angular_similarity.h b/SimoxUtility/math/vector_similarity/angular_similarity.h index 5251ed57f0e0bb76605002c79e9cd3051f648f7e..8d1978682607ac485ff8c90efdb2ddf6f1896240 100644 --- a/SimoxUtility/math/vector_similarity/angular_similarity.h +++ b/SimoxUtility/math/vector_similarity/angular_similarity.h @@ -30,10 +30,13 @@ namespace simox::math angular_similarity( const Eigen::Matrix<float, rows, 1>& v1, const Eigen::Matrix<float, rows, 1>& v2) - noexcept { - const float angular_distance = std::acos(cosine_similarity(v1, v2)) / M_PI; - return 1.f - angular_distance; + const float cosine_similarity = math::cosine_similarity(v1, v2); + const float angular_distance = std::acos(cosine_similarity) / M_PI; + const float angular_similarity = 1.f - angular_distance; + + // Clamp to deal with numerical inaccuracies. + return std::clamp(angular_similarity, 0.f, 1.f); } } diff --git a/SimoxUtility/math/vector_similarity/cosine_similarity.h b/SimoxUtility/math/vector_similarity/cosine_similarity.h index 1fd1619eed503b06a8561263b26876daddede67a..cbe5138c3245e3bc2d16f1d13ee12cd413059189 100644 --- a/SimoxUtility/math/vector_similarity/cosine_similarity.h +++ b/SimoxUtility/math/vector_similarity/cosine_similarity.h @@ -34,7 +34,10 @@ namespace simox::math throw std::logic_error{"Cosine similarity is not defined on operands which are zero."}; } - return v1.dot(v2) / (v1.norm() * v2.norm()); + const float cosine_similarity = v1.dot(v2) / (v1.norm() * v2.norm()); + + // Clamp to deal with numerical inaccuracies. + return std::clamp(cosine_similarity, -1.f, 1.f); } }