fixed the (*) to -> and code some more code beautification.

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 << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
-    (*H) /= pow(x2 + y2 + z2, 1.5);
+    *H /= pow(x2 + y2 + z2, 1.5);
   }
   return normalized;
 }

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 (H1) H1->setIdentity();
-      if (H2) (*H2).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(H1) *H1 = _I_3x3;
-      if(H2) (*H2).setIdentity();
+      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;
     }

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,2>(0,0) = R; // [R R_{pi/2}q]
-        (*H1).block<2,1>(0,2) = Drotate1;
+        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->setZero();
-    (*H2).block<1,2>(0,0) = H2_;
+    H2->block<1,2>(0,0) = H2_;
   }
   return result;
 }

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->setZero();
-    (*H2).block<1,3>(0,3) = H2_;
+    H2->block<1,3>(0,3) = H2_;
   }
   return r;
 }

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) {
+    if (H) *H << 0.0, 0.0;
-      (*H) << 0.0, 0.0;
-    }
     return Rot2();
   }
 }

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)
+  if (Hp) *Hp << q.basis().transpose() * matrix() * Dp;
-    (*Hp) = q.basis().transpose() * matrix() * Dp;
+  if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
-  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)
+  if (Hp) *Hp << q.basis().transpose() * matrix().transpose () * Dp;
-    (*Hp) = q.basis().transpose() * matrix().transpose () * Dp;
+  if (HR) *HR = q.basis().transpose() * q.skew();
-  if (HR)
-    (*HR) = q.basis().transpose() * q.skew();
   return q;
 }