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

10
gtsam/geometry/Rot2.cpp
View File

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

3
gtsam/geometry/Rot2.h
View File

@ -51,6 +51,9 @@ namespace gtsam {
/** default constructor, zero rotation */ /** default constructor, zero rotation */
Rot2() : c_(1.0), s_(0.0) {} 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 /// Constructor from angle in radians == exponential map at identity
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}