diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index d82faf971..152b5566b 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) - (*H1) = I_3x3; + *H1 = I_3x3; if (H2) - (*H2) = I_3x3; + *H2 = I_3x3; return *this - q; } @@ -112,8 +112,8 @@ Point3 Point3::normalize(OptionalJacobian<3,3> H) const { // 3*3 Derivative double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - (*H) << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; - (*H) /= pow(x2 + y2 + z2, 1.5); + *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; + *H /= pow(x2 + y2 + z2, 1.5); } return normalized; } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 33d9e47a5..0cd038d90 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -95,8 +95,8 @@ namespace gtsam { inline Point3 compose(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if (H1) (*H1).setIdentity(); - if (H2) (*H2).setIdentity(); + if (H1) H1->setIdentity(); + if (H2) H2->setIdentity(); return *this + p2; } @@ -107,8 +107,8 @@ namespace gtsam { inline Point3 between(const Point3& p2, OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const { - if(H1) (*H1) = (-Eigen::Matrix3d::Identity()); - if(H2) (*H2).setIdentity(); + if(H1) *H1 = _I_3x3; + if(H2) H2->setIdentity(); return p2 - *this; } @@ -167,12 +167,12 @@ namespace gtsam { double d = (p2 - *this).norm(); if (H1) { *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); - (*H1) = (*H1) *(1./d); + *H1 = *H1 *(1./d); } if (H2) { *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); - (*H2) = (*H2) *(1./d); + *H2 = *H2 *(1./d); } return d; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 83895ad7c..bf6f02e17 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -163,8 +163,8 @@ Point2 Pose2::transform_from(const Point2& p, Matrix21 Drotate1; Drotate1 << -q.y(), q.x(); if (H1) { - (*H1).block<2,2>(0,0) = R; // [R R_{pi/2}q] - (*H1).block<2,1>(0,2) = Drotate1; + H1->block<2,2>(0,0) = R; // [R R_{pi/2}q] + H1->block<2,1>(0,2) = Drotate1; } if (H2) *H2 = R; // R } @@ -226,8 +226,8 @@ Rot2 Pose2::bearing(const Pose2& pose, Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { Matrix12 H2_ = D2 * pose.r().matrix(); - (*H2).setZero(); - (*H2).block<1,2>(0,0) = H2_; + H2->setZero(); + H2->block<1,2>(0,0) = H2_; } return result; } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2e241a1e1..35abf39a2 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -65,13 +65,13 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, OptionalJacobian<6,6> H) { if (H) { - (*H).setZero(); + H->setZero(); for (int i = 0; i < 6; ++i) { Vector6 dxi; dxi.setZero(); dxi(i) = 1.0; Matrix6 Gi = adjointMap(dxi); - (*H).col(i) = Gi * y; + H->col(i) = Gi * y; } } return adjointMap(xi) * y; @@ -87,7 +87,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, dxi.setZero(); dxi(i) = 1.0; Matrix6 GTi = adjointMap(dxi).transpose(); - (*H).col(i) = GTi * y; + H->col(i) = GTi * y; } } return adjointMap(xi).transpose() * y; @@ -297,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - (*H2) = I_6x6; + *H2 = I_6x6; return result; } @@ -327,8 +327,8 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { Matrix13 H2_ = D2 * pose.rotation().matrix(); - (*H2).setZero(); - (*H2).block<1,3>(0,3) = H2_; + H2->setZero(); + H2->block<1,3>(0,3) = H2_; } return r; } diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index af8d701ec..13c22ddc1 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -102,13 +102,11 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { if (H) { - (*H) << -y / d2, x / d2; + *H << -y / d2, x / d2; } return Rot2::fromCosSin(x / n, y / n); } else { - if (H) { - (*H) << 0.0, 0.0; - } + if (H) *H << 0.0, 0.0; return Rot2(); } } diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 95e7f85ce..5d7a2a473 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -73,10 +73,8 @@ Unit3 Rot3::rotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * Dp; - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); + if (Hp) *Hp << q.basis().transpose() * matrix() * Dp; + if (HR) *HR = -q.basis().transpose() * matrix() * p.skew(); return q; } @@ -85,10 +83,8 @@ Unit3 Rot3::unrotate(const Unit3& p, OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { Matrix32 Dp; Unit3 q = Unit3(unrotate(p.point3(Dp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix().transpose () * Dp; - if (HR) - (*HR) = q.basis().transpose() * q.skew(); + if (Hp) *Hp << q.basis().transpose() * matrix().transpose () * Dp; + if (HR) *HR = q.basis().transpose() * q.skew(); return q; }