`EdgePosition` Jacobians are incorrect

[JeffLIrion]

Also, it's not necessary to add on the zeros.

I've made a bunch of improvements to the graphslam package, I suggest updating to the latest version.

The code below works for PoseR2, PoseR3, PoseSE2, and PoseSE3 vertices.

class EdgePosition(BaseEdge):

    def calc_error(self):
        return (self.estimate - (self.vertices[1].pose - self.vertices[0].pose)).position

    def calc_jacobians(self):
        dim_point = len(self.vertices[0].pose.position)
        return [np.dot(np.dot(self.estimate.jacobian_self_ominus_other_wrt_other_compact(self.vertices[1].pose - self.vertices[0].pose), self.vertices[1].pose.jacobian_self_ominus_other_wrt_other(self.vertices[0].pose))[:dim_point, :], self.vertices[0].pose.jacobian_boxplus()),
                np.dot(np.dot(self.estimate.jacobian_self_ominus_other_wrt_other_compact(self.vertices[1].pose - self.vertices[0].pose), self.vertices[1].pose.jacobian_self_ominus_other_wrt_self(self.vertices[0].pose))[:dim_point, :], self.vertices[1].pose.jacobian_boxplus())]

[JeffLIrion]

Here are unit tests that the code above passes.

Also, you don't have to implement the calc_jacobians() method for a custom edge type. If you don't, it will use numerical differentiation (which is what the unit tests below use to verify that the analytical Jacobians above are correct).

class TestEdgePosition(unittest.TestCase):
    """Tests for the ``EdgePosition`` class."""

    def test_calc_jacobians_r2(self):
        """Test that the ``calc_jacobians`` method gives the correct results."""
        np.random.seed(0)

        for _ in range(10):
            p1 = PoseR2(np.random.random_sample(2))
            p2 = PoseR2(np.random.random_sample(2))
            estimate = PoseR2(np.random.random_sample(2))

            v1 = Vertex(1, p1)
            v2 = Vertex(2, p2)

            e = EdgePosition([1, 2], np.eye(2), estimate, [v1, v2])

            numerical_jacobians = BaseEdge.calc_jacobians(e)

            analytical_jacobians = e.calc_jacobians()

            self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
            for n, a in zip(numerical_jacobians, analytical_jacobians):
                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0, places=5)

    def test_calc_jacobians_r3(self):
        """Test that the ``calc_jacobians`` method gives the correct results."""
        np.random.seed(0)

        for _ in range(10):
            p1 = PoseR3(np.random.random_sample(3))
            p2 = PoseR3(np.random.random_sample(3))
            estimate = PoseR3(np.random.random_sample(3))

            v1 = Vertex(1, p1)
            v2 = Vertex(2, p2)

            e = EdgePosition([1, 2], np.eye(3), estimate, [v1, v2])

            numerical_jacobians = BaseEdge.calc_jacobians(e)

            analytical_jacobians = e.calc_jacobians()

            self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
            for n, a in zip(numerical_jacobians, analytical_jacobians):
                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0, places=5)

    def test_calc_jacobians_se2(self):
        """Test that the ``calc_jacobians`` method gives the correct results."""
        np.random.seed(0)

        for _ in range(10):
            p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
            p2 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
            estimate = PoseSE2(np.random.random_sample(2), np.random.random_sample())

            v1 = Vertex(1, p1)
            v2 = Vertex(2, p2)

            e = EdgePosition([1, 2], np.eye(2), estimate, [v1, v2])

            numerical_jacobians = BaseEdge.calc_jacobians(e)

            analytical_jacobians = e.calc_jacobians()

            self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
            for n, a in zip(numerical_jacobians, analytical_jacobians):
                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0, places=5)

    def test_calc_jacobians_se3(self):
        """Test that the ``calc_jacobians`` method gives the correct results."""
        np.random.seed(0)

        for _ in range(10):
            p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
            p2 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
            estimate = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))

            p1.normalize()
            p2.normalize()
            estimate.normalize()

            v1 = Vertex(1, p1)
            v2 = Vertex(2, p2)

            e = EdgePosition([1, 2], np.eye(3), estimate, [v1, v2])

            self.assertAlmostEqual(np.linalg.norm(e.calc_error_new() - e.calc_error_original()), 0.0)

            numerical_jacobians = BaseEdge.calc_jacobians(e)

            analytical_jacobians = e.calc_jacobians()

            self.assertEqual(len(numerical_jacobians), len(analytical_jacobians))
            for n, a in zip(numerical_jacobians, analytical_jacobians):
                self.assertAlmostEqual(np.linalg.norm(n - a), 0.0, places=5)