Pose3::between now 4 times faster (in Debug, did not test in Release)
							parent
							
								
									b40db06bf5
								
							
						
					
					
						commit
						620583f3c3
					
				|  | @ -226,11 +226,9 @@ namespace gtsam { | |||
|   // between = compose(p2,inverse(p1));
 | ||||
|   Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1, | ||||
|       boost::optional<Matrix&> H2) const { | ||||
|       Matrix invH; | ||||
|     Pose3 invp1 = inverse(invH); | ||||
|     Matrix composeH1; | ||||
|     Pose3 result = invp1.compose(p2, composeH1, H2); | ||||
|     if (H1) *H1 = composeH1 * invH; | ||||
|     Pose3 result = inverse()*p2; | ||||
|     if (H1) *H1 = -result.inverse().adjointMap(); | ||||
|     if (H2) *H2 = I6; | ||||
|     return result; | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -40,10 +40,13 @@ int main() | |||
|   double x=1.0/norm, y=4.0/norm, z=2.0/norm; | ||||
|   Vector v = Vector_(6,x,y,z,0.1,0.2,-0.1); | ||||
|   Pose3 T = Pose3::Expmap(Vector_(6,0.1,0.1,0.2,0.1, 0.4, 0.2)), T2 = T.retract(v); | ||||
|   Matrix H1,H2; | ||||
| 
 | ||||
|   TEST("retract", T.retract(v)) | ||||
|   TEST("Expmap", T*Pose3::Expmap(v)) | ||||
|   TEST("localCoordinates", T.localCoordinates(T2)) | ||||
|   TEST("between", T.between(T2)) | ||||
|   TEST("between-derivatives", T.between(T2,H1,H2)) | ||||
|   TEST("Logmap", Pose3::Logmap(T.between(T2))) | ||||
| 
 | ||||
|   return 0; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue