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

More efficient transform_to using unrotate

release/4.3a0
Frank Dellaert 2009-08-29 04:10:52 +00:00
parent 3432b6c50f
commit 5eb9a4d182
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/Pose3.cpp
View File

@ -33,8 +33,8 @@ Matrix Dtransform_from2(const Pose3& pose) {
/* ************************************************************************* */ /* ************************************************************************* */
Point3 transform_to(const Pose3& pose, const Point3& p) { Point3 transform_to(const Pose3& pose, const Point3& p) {
Point3 sub = p - pose.translation(); Point3 sub = p - pose.t_;
Point3 r = rotate(pose.rotation().inverse(), sub); Point3 r = unrotate(pose.R_, sub);
return r; return r;
} }
/* ************************************************************************* */ /* ************************************************************************* */