Merge pull request #1511 from zubingtan/fix_line3_transform_to_jacobian_of_line
Fix wrong jacobian to line in Line3::transformTorelease/4.3a0
commit
b32c20b48a
|
@ -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]);
|
||||
}
|
||||
|
|
|
@ -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)));
|
||||
|
||||
|
|
Loading…
Reference in New Issue