fix Pose2Prior problem and adding prior factor when splitting factor graph
							parent
							
								
									2b82ff65e7
								
							
						
					
					
						commit
						6a7987fa91
					
				|  | @ -337,8 +337,10 @@ void FactorGraph<Factor>::split(map<string, string> tree, FactorGraph<Factor>& A | |||
| 		if (factor->keys().size() > 2) | ||||
| 			throw(invalid_argument("split: only support factors with at most two keys")); | ||||
| 
 | ||||
| 		if (factor->keys().size() == 1) | ||||
| 		if (factor->keys().size() == 1) { | ||||
| 			Ab1.push_back(factor); | ||||
| 			continue; | ||||
| 		} | ||||
| 
 | ||||
| 		string key1 = factor->keys().front(); | ||||
| 		string key2 = factor->keys().back(); | ||||
|  |  | |||
|  | @ -33,8 +33,7 @@ public: | |||
| /* ************************************************************************* */ | ||||
| Ordering::Ordering(const map<string, string>& p_map) { | ||||
| 
 | ||||
| 	typedef boost::adjacency_list< | ||||
| 		boost::vecS, boost::vecS, boost::undirectedS, | ||||
| 	typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, | ||||
| 		boost::property<boost::vertex_name_t, string> > Graph; | ||||
| 	typedef boost::graph_traits<Graph>::vertex_descriptor Vertex; | ||||
| 
 | ||||
|  |  | |||
|  | @ -44,7 +44,7 @@ public: | |||
| 	/** implement functions needed to derive from Factor */ | ||||
| 	Vector error_vector(const Pose2Config& config) const { | ||||
| 		Pose2 p = config.get(key_); | ||||
| 		return -p.log(measured_); | ||||
| 		return -logmap(p,measured_); | ||||
| 	} | ||||
| 
 | ||||
| 	std::list<std::string> keys() const { return keys_; } | ||||
|  | @ -53,7 +53,7 @@ public: | |||
| 	/** linearize */ | ||||
| 	boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const { | ||||
| 		Pose2 p = config.get(key_); | ||||
| 		Vector b = -p.log(measured_); | ||||
| 		Vector b = logmap(p,measured_); | ||||
| 		Matrix H(3,3); | ||||
| 		H(0,0)=1; H(0,1)=0; H(0,2)=0; | ||||
| 		H(1,0)=0; H(1,1)=1; H(1,2)=0; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue