12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

normalize cos and sin when there is numerical error, which does happen sometimes when composing two rotations

release/4.3a0
Kai Ni 2010-04-29 21:28:24 +00:00
parent f81519b046
commit e2728184b9
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/FactorGraph.h
View File

@ -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<FactorGraph<Factor>, std::set<Symbol> > removeSingletons();

7
cpp/Rot2.cpp
View File

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