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

Add user-defined copy constructor for Rot2

release/4.3a0
Jay Elrod 2021-06-29 17:07:15 -04:00
parent 6ee17c2ed7
commit 7c358aae4c
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) { 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);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -59,8 +55,8 @@ 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

@ -50,6 +50,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)) {}