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)
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();

3
cpp/Ordering.cpp
View File

@ -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;

4
cpp/Pose2Prior.h
View File

@ -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;