Restored old version of between factor. Uncommented tests
							parent
							
								
									c06334af5f
								
							
						
					
					
						commit
						c43e77cdfa
					
				|  | @ -90,12 +90,12 @@ namespace gtsam { | |||
|     Vector evaluateError(const T& p1, const T& p2, | ||||
|         boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 = | ||||
|             boost::none) const { | ||||
| 
 | ||||
|       T hx = p1.between(p2, H1, H2); // h(x)
 | ||||
|       DefaultChart<T> chart; | ||||
|       // TODO check:
 | ||||
|       //T hx = p1.between(p2, H1, H2); // h(x)
 | ||||
|       T hx = chart.local(p2, p1); | ||||
|       if(H1) (*H1) = -eye(chart.getDimension(p1)); | ||||
|       if(H2) (*H2) = eye(chart.getDimension(p2)); | ||||
|       //T hx = chart.local(p2, p1);
 | ||||
|       //if(H1) (*H1) = -eye(chart.getDimension(p1));
 | ||||
|       //if(H2) (*H2) = eye(chart.getDimension(p2));
 | ||||
| 
 | ||||
|       // manifold equivalent of h(x)-z -> log(z,h(x))
 | ||||
|       return chart.local(measured_, hx); | ||||
|  |  | |||
|  | @ -45,7 +45,7 @@ TEST(BetweenFactor, Rot3) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| /*
 | ||||
| // Constructor scalar
 | ||||
| TEST(BetweenFactor, ConstructorScalar) { | ||||
|   SharedNoiseModel model; | ||||
|  | @ -66,6 +66,7 @@ TEST(BetweenFactor, ConstructorDynamicSizeVector) { | |||
|   Vector measured_value(5); measured_value << 1, 2, 3, 4, 5; | ||||
|   BetweenFactor<Vector> factor(1, 2, measured_value, model); | ||||
| } | ||||
| */ | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue