diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index acdfcf784..d2dc8d892 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -166,11 +166,11 @@ namespace gtsam { boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { double d = (p2 - *this).norm(); if (H1) { - *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z())*(1./d); + *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d); } if (H2) { - *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z())*(1./d); + *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d); } return d; }