Math fix in linearize() and error_vector()
							parent
							
								
									7161878285
								
							
						
					
					
						commit
						3c0ae0ec1c
					
				|  | @ -20,14 +20,18 @@ private: | |||
| 	std::string key1_, key2_; /** The keys of the two poses, order matters */ | ||||
| 	Pose2 measured_; | ||||
| 	Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */ | ||||
| 	std::list<std::string> keys_; | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 	typedef boost::shared_ptr<Pose2Factor> shared_ptr; // shorthand for a smart pointer to a factor
 | ||||
| 
 | ||||
| 	Pose2Factor(const std::string& key1, const std::string& key2, | ||||
| 			const Pose2& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) { | ||||
| 			const Pose2& measured, const Matrix& measurement_covariance) : | ||||
| 			  key1_(key1),key2_(key2),measured_(measured) { | ||||
| 		square_root_inverse_covariance_ = inverse_square_root(measurement_covariance); | ||||
| 		keys_.push_back(key1); | ||||
| 		keys_.push_back(key2); | ||||
| 	} | ||||
| 
 | ||||
| 	/** implement functions needed for Testable */ | ||||
|  | @ -45,16 +49,16 @@ public: | |||
| 	Vector error_vector(const Pose2Config& config) const { | ||||
| 		//z-h
 | ||||
| 		Pose2 p1 = config.get(key1_), p2 = config.get(key2_); | ||||
| 		return (measured_ - between(p1,p2)).vector(); | ||||
| 		return -between(p1,p2).log(measured_); | ||||
| 	} | ||||
| 
 | ||||
| 	std::list<std::string> keys() const { std::list<std::string> l; return l; } | ||||
| 	std::size_t size() const { return 2;} | ||||
| 	std::list<std::string> keys() const { return keys_; } | ||||
| 	std::size_t size() const { return keys_.size(); } | ||||
| 
 | ||||
| 	/** linearize */ | ||||
| 	boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const { | ||||
| 		Pose2 p1 = config.get(key1_), p2 = config.get(key2_); | ||||
| 		Vector b = (measured_ - between(p1,p2)).vector(); | ||||
| 		Vector b = -between(p1,p2).log(measured_); | ||||
| 		Matrix H1 = Dbetween1(p1,p2); | ||||
| 		Matrix H2 = Dbetween2(p1,p2); | ||||
| 		boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor( | ||||
|  |  | |||
|  | @ -28,7 +28,7 @@ TEST( Pose2Factor, constructor ) | |||
| 
 | ||||
| 	// Choose a linearization point
 | ||||
| 	Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
 | ||||
| 	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4) looking at negative (ground truth is at 4.1,2)
 | ||||
| 	Pose2 p2(-1,4.1,M_PI);  // robot at (-1,4.1) looking at negative (ground truth is at 4.1,2)
 | ||||
| 	Pose2Config config; | ||||
| 	config.insert("p1",p1); | ||||
| 	config.insert("p2",p2); | ||||
|  | @ -59,11 +59,12 @@ TEST( Pose2Factor, constructor ) | |||
| 	GaussianFactor expected( | ||||
| 			"p1", square_root_inverse_covariance*expectedH1, | ||||
| 			"p2", square_root_inverse_covariance*expectedH2, | ||||
| 			Vector_(3,-0.1,-0.1,0.0), 1.0); | ||||
| 			Vector_(3,0.1,0.1,0.0), 1.0); | ||||
| 
 | ||||
| 	CHECK(assert_equal(expected,*actual)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
| 	TestResult tr; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue