diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index ba4e4b752..42d8954b3 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -337,8 +337,10 @@ void FactorGraph::split(map tree, FactorGraph& 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(); diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 8e0c23370..4e5a4e2bb 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -33,8 +33,7 @@ public: /* ************************************************************************* */ Ordering::Ordering(const map& p_map) { - typedef boost::adjacency_list< - boost::vecS, boost::vecS, boost::undirectedS, + typedef boost::adjacency_list > Graph; typedef boost::graph_traits::vertex_descriptor Vertex; diff --git a/cpp/Pose2Prior.h b/cpp/Pose2Prior.h index 9b3c3933e..24055d222 100644 --- a/cpp/Pose2Prior.h +++ b/cpp/Pose2Prior.h @@ -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 keys() const { return keys_; } @@ -53,7 +53,7 @@ public: /** linearize */ boost::shared_ptr 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;