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