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

fixed some issues in SE(2) "branch"

release/4.3a0
Frank Dellaert 2010-03-03 05:34:08 +00:00
parent 707627fb3a
commit c9fcf95501
2 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/Pose2.cpp
View File

@ -153,7 +153,7 @@ namespace gtsam {
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::fromCosSin(c,s));
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - p1.t();

2
cpp/Pose2.h
View File

@ -52,7 +52,7 @@ namespace gtsam {
/** Constructor from 3*3 matrix */
Pose2(const Matrix &T) :
r_(Rot2::fromCosSin(T(0, 0), T(1, 0))), t_(T(0, 2), T(1, 2)) {}
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {}
/** print with optional string */
void print(const std::string& s = "") const;