From e2728184b98b3e015de31411c60045cd20e1ba73 Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Thu, 29 Apr 2010 21:28:24 +0000 Subject: [PATCH] normalize cos and sin when there is numerical error, which does happen sometimes when composing two rotations --- cpp/FactorGraph.h | 2 -- cpp/Rot2.cpp | 7 +++++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index fb0193c75..f206f0be3 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -100,8 +100,6 @@ namespace gtsam { return !(indices_.find(key)==indices_.end()); } - /** check whether a variable is a singleton, i.e. it only involve*/ - /** remove singleton variables and the related factors */ std::pair, std::set > removeSingletons(); diff --git a/cpp/Rot2.cpp b/cpp/Rot2.cpp index 260e0edb9..03249e17d 100644 --- a/cpp/Rot2.cpp +++ b/cpp/Rot2.cpp @@ -17,8 +17,11 @@ namespace gtsam { /* ************************************************************************* */ Rot2 Rot2::fromCosSin(double c, double s) { - if (fabs(c * c + s * s - 1.0) > 1e-9) throw std::invalid_argument( - "Rot2::fromCosSin: needs cos/sin pair"); + if (fabs(c * c + s * s - 1.0) > 1e-9) { + double norm_cs = sqrt(c*c + s*s); + c = c/norm_cs; + s = s/norm_cs; + } return Rot2(c, s); }