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

replace double matrix call

release/4.3a0
Frank Dellaert 2012-01-04 16:50:55 +00:00
parent 2572265096
commit 2dde2cf27c
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/Rot3M.cpp
View File

@ -183,8 +183,11 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = matrix();
if (H1 || H2) {
const Matrix R(matrix());
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
}
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
}