Fixed double map lookup
							parent
							
								
									769763a35d
								
							
						
					
					
						commit
						bea55b5f5b
					
				|  | @ -63,14 +63,18 @@ namespace gtsam { | |||
| 		/** implement functions needed to derive from Factor */ | ||||
| 
 | ||||
| 		/** vector of errors */ | ||||
| 		Vector error_vector(const Config& x) const { | ||||
| 		Vector error_vector(const T& p1, const T& p2) const { | ||||
| 			//z-h
 | ||||
| 			const T& p1 = x.get(key1_), p2 = x.get(key2_); | ||||
| 			T hx = between(p1,p2); | ||||
| 			// manifold equivalent of z-h(x) -> log(h(x),z)
 | ||||
| 			return square_root_inverse_covariance_ * logmap(hx,measured_); | ||||
| 		} | ||||
| 
 | ||||
| 		/** vector of errors */ | ||||
| 		Vector error_vector(const Config& c) const { | ||||
| 		  return error_vector(c.get(key1_), c.get(key2_)); | ||||
| 		} | ||||
| 
 | ||||
| 		/** methods to retrieve both keys */ | ||||
| 		inline const std::string& key1() const { return key1_;} | ||||
| 		inline const std::string& key2() const { return key2_;} | ||||
|  | @ -86,7 +90,7 @@ namespace gtsam { | |||
| 			const T& p1 = x0.get(key1_), p2 = x0.get(key2_); | ||||
| 			Matrix A1 = Dbetween1(p1, p2); | ||||
| 			Matrix A2 = Dbetween2(p1, p2); | ||||
| 			Vector b = error_vector(x0); // already has sigmas in !
 | ||||
| 			Vector b = error_vector(p1, p2); // already has sigmas in !
 | ||||
| 			boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor( | ||||
| 					key1_, square_root_inverse_covariance_ * A1, | ||||
| 					key2_, square_root_inverse_covariance_ * A2, b, 1.0)); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue