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); }