12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1511 from zubingtan/fix_line3_transform_to_jacobian_of_line 

Fix wrong jacobian to line in Line3::transformTo
release/4.3a0
Frank Dellaert 2023-04-13 08:30:14 -07:00 committed by GitHub
parent 856ff94d70 9279d2713f
commit b32c20b48a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Line3.cpp
View File

@ -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]);
}

4
gtsam/geometry/tests/testLine3.cpp
View File

@ -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)));