From 9279d2713f724933743947f834f7b1af3c427195 Mon Sep 17 00:00:00 2001
From: zubingtan <zubingtan@pony.ai>
Date: Thu, 13 Apr 2023 14:29:18 +0800
Subject: [PATCH] fix jacobian to line in Line3::transformTo

---
 gtsam/geometry/Line3.cpp           | 4 ++--
 gtsam/geometry/tests/testLine3.cpp | 4 ++--
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp
index 9e7b2e13e..f5cf344f5 100644
--- a/gtsam/geometry/Line3.cpp
+++ b/gtsam/geometry/Line3.cpp
@@ -111,8 +111,8 @@ Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
   }
   if (Dline) {
     Dline->setIdentity();
-    (*Dline)(0, 3) = -t[2];
-    (*Dline)(1, 2) = t[2];
+    (*Dline)(3, 0) = -t[2];
+    (*Dline)(2, 1) = t[2];
   }
   return Line3(cRl, c_ab[0], c_ab[1]);
 }
diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp
index 09371bad4..ae2a5e05d 100644
--- a/gtsam/geometry/tests/testLine3.cpp
+++ b/gtsam/geometry/tests/testLine3.cpp
@@ -123,10 +123,10 @@ TEST(Line3, localCoordinatesOfRetract) {
 // transform from world to camera test
 TEST(Line3, transformToExpressionJacobians) {
   Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0));
-  Vector3 t(0, 0, 0);
+  Vector3 t(-2.0, 2.0, 3.0);
   Pose3 p(r, t);
 
-  Line3 l_c(r.inverse(), 1, 1);
+  Line3 l_c(r.inverse(), 3, -1);
   Line3 l_w(Rot3(), 1, 1);
   EXPECT(l_c.equals(transformTo(p, l_w)));