12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix Pose2Prior problem and adding prior factor when splitting factor graph

release/4.3a0
Kai Ni 2010-01-09 23:39:11 +00:00
parent 2b82ff65e7
commit 6a7987fa91
3 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cpp/FactorGraph-inl.h
View File

@ -337,8 +337,10 @@ void FactorGraph<Factor>::split(map<string, string> tree, FactorGraph<Factor>& A
if (factor->keys().size() > 2) if (factor->keys().size() > 2)
throw(invalid_argument("split: only support factors with at most two keys")); 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; continue;
}
string key1 = factor->keys().front(); string key1 = factor->keys().front();
string key2 = factor->keys().back(); string key2 = factor->keys().back();

3
cpp/Ordering.cpp
View File

@ -33,8 +33,7 @@ public:
/* ************************************************************************* */ /* ************************************************************************* */
Ordering::Ordering(const map<string, string>& p_map) { Ordering::Ordering(const map<string, string>& p_map) {
typedef boost::adjacency_list< typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
boost::vecS, boost::vecS, boost::undirectedS,
boost::property<boost::vertex_name_t, string> > Graph; boost::property<boost::vertex_name_t, string> > Graph;
typedef boost::graph_traits<Graph>::vertex_descriptor Vertex; typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;

4
cpp/Pose2Prior.h
View File

@ -44,7 +44,7 @@ public:
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
Vector error_vector(const Pose2Config& config) const { Vector error_vector(const Pose2Config& config) const {
Pose2 p = config.get(key_); Pose2 p = config.get(key_);
return -p.log(measured_); return -logmap(p,measured_);
} }
std::list<std::string> keys() const { return keys_; } std::list<std::string> keys() const { return keys_; }
@ -53,7 +53,7 @@ public:
/** linearize */ /** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const { boost::shared_ptr<GaussianFactor> linearize(const Pose2Config& config) const {
Pose2 p = config.get(key_); Pose2 p = config.get(key_);
Vector b = -p.log(measured_); Vector b = logmap(p,measured_);
Matrix H(3,3); Matrix H(3,3);
H(0,0)=1; H(0,1)=0; H(0,2)=0; H(0,0)=1; H(0,1)=0; H(0,2)=0;
H(1,0)=0; H(1,1)=1; H(1,2)=0; H(1,0)=0; H(1,1)=1; H(1,2)=0;