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

Merge pull request #806 from jaelrod/fix/rot2-copy-ctor 

Add user-defined copy constructor for Rot2
release/4.3a0
Frank Dellaert 2021-06-30 14:41:40 -04:00 committed by GitHub
parent 6ee17c2ed7 7c358aae4c
commit 6d6d129d05
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 7 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Rot2.cpp
View File

@ -25,12 +25,8 @@ namespace gtsam {
/* ************************************************************************* */
Rot2 Rot2::fromCosSin(double c, double s) {
if (std::abs(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);
Rot2 R(c, s);
return R.normalize();
}
/* ************************************************************************* */
@ -59,8 +55,8 @@ bool Rot2::equals(const Rot2& R, double tol) const {
/* ************************************************************************* */
Rot2& Rot2::normalize() {
double scale = c_*c_ + s_*s_;
if(std::abs(scale-1.0)>1e-10) {
scale = pow(scale, -0.5);
if(std::abs(scale-1.0) > 1e-10) {
scale = 1 / sqrt(scale);
c_ *= scale;
s_ *= scale;
}

3
gtsam/geometry/Rot2.h
View File

@ -50,6 +50,9 @@ namespace gtsam {
/** default constructor, zero rotation */
Rot2() : c_(1.0), s_(0.0) {}
/** copy constructor */
Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
/// Constructor from angle in radians == exponential map at identity
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}