Add analytical Jacobian for EdgeSE2PointXYBearing type (#818)

* Add analytical Jacobian for EdgeSE2PointXYBearing type

Author: matthew-t-watson

g2o/types/slam2d/edge_se2_pointxy_bearing.cpp

@@ -52,6 +52,26 @@ void EdgeSE2PointXYBearing::initialEstimate(
   l2->setEstimate(t * vr);
 }
 
+#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
+void EdgeSE2PointXYBearing::linearizeOplus() {
+  const VertexSE2* vi = static_cast<const VertexSE2*>(_vertices[0]);
+  const VertexPointXY* vj = static_cast<const VertexPointXY*>(_vertices[1]);
+  const double& x1 = vi->estimate().translation()[0];
+  const double& y1 = vi->estimate().translation()[1];
+  const double& x2 = vj->estimate()[0];
+  const double& y2 = vj->estimate()[1];
+
+  double aux = (std::pow(x2 - x1, 2) + std::pow(y2 - y1, 2));
+
+  _jacobianOplusXi(0, 0) = (y1 - y2) / aux;
+  _jacobianOplusXi(0, 1) = (x2 - x1) / aux;
+  _jacobianOplusXi(0, 2) = 1;
+
+  _jacobianOplusXj(0, 0) = (y2 - y1) / aux;
+  _jacobianOplusXj(0, 1) = (x1 - x2) / aux;
+}
+#endif
+
 bool EdgeSE2PointXYBearing::read(std::istream& is) {
   is >> _measurement >> information()(0, 0);
   return true;

g2o/types/slam2d/edge_se2_pointxy_bearing.h

@@ -77,6 +77,10 @@ class G2O_TYPES_SLAM2D_API EdgeSE2PointXYBearing
   }
   virtual void initialEstimate(const OptimizableGraph::VertexSet& from,
                                OptimizableGraph::Vertex* to);
+
+#ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
+  virtual void linearizeOplus();
+#endif
 };
 
 class G2O_TYPES_SLAM2D_API EdgeSE2PointXYBearingWriteGnuplotAction

unit_test/slam2d/jacobians_slam2d.cpp

@@ -156,10 +156,17 @@ TEST(Slam2D, EdgeSE2PointXYBearingJacobian) {
   numericJacobianWorkspace.allocate();
 
   for (int k = 0; k < 10000; ++k) {
-    v1.setEstimate(randomSE2());
-    v2.setEstimate(Eigen::Vector2d::Random());
-    e.setMeasurement(g2o::Sampler::uniformRand(0., 1.) * M_PI);
-
-    evaluateJacobian(e, jacobianWorkspace, numericJacobianWorkspace);
+    /* Generate random estimate states, but don't evaluate those that are too
+     * close to error function singularity. */
+    do {
+      v1.setEstimate(randomSE2());
+      v2.setEstimate(Eigen::Vector2d::Random());
+      e.setMeasurement(g2o::Sampler::uniformRand(-1., 1.) * M_PI);
+    } while ((v1.estimate().inverse() * v2.estimate()).norm() < 1e-6);
+
+    /* Note a larger tolerance versus the default of 1e-6 must be used due to
+     * poor behaviour of the numerical difference function that is used to
+     * provide golden data. */
+    evaluateJacobian(e, jacobianWorkspace, numericJacobianWorkspace, 1e-5);
   }
 }

unit_test/test_helper/evaluate_jacobian.h

@@ -63,7 +63,8 @@ void evaluateJacobianUnary(EdgeType& e, JacobianWorkspace& jacobianWorkspace,
 
 template <typename EdgeType>
 void evaluateJacobian(EdgeType& e, JacobianWorkspace& jacobianWorkspace,
-                      JacobianWorkspace& numericJacobianWorkspace) {
+                      JacobianWorkspace& numericJacobianWorkspace,
+                      double tolerance = 1e-6) {
   // calling the analytic Jacobian but writing to the numeric workspace
   e.BaseBinaryEdge<EdgeType::Dimension, typename EdgeType::Measurement,
                    typename EdgeType::VertexXiType,
@@ -88,7 +89,7 @@ void evaluateJacobian(EdgeType& e, JacobianWorkspace& jacobianWorkspace,
     else
       numElems *= EdgeType::VertexXjType::Dimension;
     for (int j = 0; j < numElems; ++j) {
-      EXPECT_NEAR(n[j], a[j], 1e-6);
+      EXPECT_NEAR(n[j], a[j], tolerance);
     }
   }
 }