From 930c77642ee55f4f6c30313e07b9fa66d685870a Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Oct 2014 12:12:54 +0200 Subject: [PATCH] Made tarnsform_to derivatives about twice as fast. Biggest culprit is still malloc. --- gtsam/geometry/Pose3.cpp | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index bfd2fcb9a..ea04c1d44 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -256,16 +256,23 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { - const Point3 result = R_.unrotate(p - t_); + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + Matrix3 Rt(R_.transpose()); + const Point3 q(Rt*(p - t_).vector()); if (Dpose) { - const Point3& q = result; - Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); + double wx = q.x(); + double wy = q.y(); + double wz = q.z(); Dpose->resize(3, 6); - (*Dpose) << DR, _I3; + (*Dpose) << + 0.0, -wz, +wy,-1.0, 0.0, 0.0, + +wz, 0.0, -wx, 0.0,-1.0, 0.0, + -wy, +wx, 0.0, 0.0, 0.0,-1.0; } if (Dpoint) - *Dpoint = R_.transpose(); - return result; + *Dpoint = Rt; + return q; } /* ************************************************************************* */