From b21db08ec12b430f3b64ab25b050131b6f929e0c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 2 Nov 2014 13:03:36 +0100 Subject: [PATCH] Fixed small issue, should not assign to reference in case of Quaternions. --- gtsam/geometry/Pose3.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index faf5d4bbb..7c4ac34e2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { if (Dpose) { - const Matrix R = R_.matrix(); - Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + const Matrix3 R = R_.matrix(); + Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Dpose->resize(3, 6); (*Dpose) << DR, R; } @@ -263,7 +263,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case - const Matrix3& Rt = R_.transpose(); + const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -280,7 +280,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { - const Matrix3& Rt = R_.transpose(); + const Matrix3 Rt = R_.transpose(); const Point3 q(Rt*(p - t_).vector()); if (Dpose) { const double wx = q.x(), wy = q.y(), wz = q.z();