Fixing issues with boost::assign

gtsam/inference/BayesTreeCliqueBase-inst.h

@@ -160,7 +160,8 @@ namespace gtsam {
         gttic(BayesTreeCliqueBase_separatorMarginal);
         gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
         // now add the parent conditional
-        p_Cp += parent->conditional_; // P(Fp|Sp)
+//        p_Cp += parent->conditional_; // P(Fp|Sp) // FIXME: assign error?
+        p_Cp.push_back(parent->conditional_);
 
         // The variables we want to keepSet are exactly the ones in S
         std::vector<Key> indicesS(this->conditional()->beginParents(), this->conditional()->endParents());

gtsam/inference/ISAM-inst.h

@@ -41,7 +41,8 @@ namespace gtsam {
 
     // Add the orphaned subtrees
     BOOST_FOREACH(const sharedClique& orphan, orphans)
-      factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
+      factors.push_back(boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan));
+//      factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
 
     // eliminate into a Bayes net
     const VariableIndex varIndex(factors);

gtsam/linear/HessianFactor.cpp

@@ -168,13 +168,26 @@ GaussianFactor(cref_list_of<3>(j1)(j2)(j3)),
 }
 
 /* ************************************************************************* */
-namespace { DenseIndex _getSizeHF(const Vector& m) { return m.size(); } }
+namespace {
+DenseIndex _getSizeHF(const Vector& m) { return m.size(); }
+}
 
 /* ************************************************************************* */
 HessianFactor::HessianFactor(const std::vector<Key>& js, const std::vector<Matrix>& Gs,
         const std::vector<Vector>& gs, double f) :
-GaussianFactor(js), info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1)))
+GaussianFactor(js) //, info_(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1)))
 {
+
+//  // FIXME: Issue with boost::range::join here - replacing with simpler version
+  info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), std::vector<DenseIndex>(1,1)));
+//  info_ = SymmetricBlockMatrix(br::join(gs | br::transformed(&_getSizeHF), cref_list_of<1,DenseIndex>(1))); // Original
+  // Alternate implementation
+//  std::vector<DenseIndex> dims(gs.size() + 1, 1);
+//  size_t i=0;
+//  BOOST_FOREACH(const Vector& m, gs)
+//    dims[i++] = m.size();
+//  info_ = SymmetricBlockMatrix(dims);
+
   // Get the number of variables
   size_t variable_count = js.size();
 

gtsam/linear/JacobianFactor-inl.h

@@ -82,9 +82,19 @@ namespace gtsam {
     // a single '1' to add a dimension for the b vector.
     {
       using boost::adaptors::transformed;
-      using boost::assign::cref_list_of;
+//      using boost::assign::cref_list_of;
       namespace br = boost::range;
-      Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size());
+      // FIXME: compile error with type mismatch in boost::join
+//      Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), cref_list_of<1,DenseIndex>(1)), b.size()); // Original
+      Ab_ = VerticalBlockMatrix(br::join(terms | transformed(&_getColsJF), std::vector<DenseIndex>(1,1)), b.size());
+
+//      // Simple implementation
+//      std::vector<DenseIndex> dims(terms.size() + 1, 1);
+//      size_t i=0;
+//      typedef std::pair<Key,Matrix> KeyMatrixPair;
+//      BOOST_FOREACH(const KeyMatrixPair& p, terms)
+//        dims[i++] = p.second.cols();
+//      Ab_ = VerticalBlockMatrix(dims, b.size());
     }
 
     // Check and add terms

gtsam/linear/JacobianFactor.cpp

@@ -265,7 +265,8 @@ namespace gtsam {
 
     // Allocate matrix and copy keys in order
     gttic(allocate);
-    Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix
+    Ab_ = VerticalBlockMatrix(boost::join(varDims, std::vector<DenseIndex>(1,1)), m); // Allocate augmented matrix
+//    Ab_ = VerticalBlockMatrix(boost::join(varDims, cref_list_of<1,DenseIndex>(1)), m); // Allocate augmented matrix - original
     Base::keys_.resize(orderedSlots.size());
     boost::range::copy(    // Get variable keys
       orderedSlots | boost::adaptors::indirected | boost::adaptors::map_keys, Base::keys_.begin());

gtsam/linear/KalmanFilter.cpp

@@ -57,12 +57,14 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  KalmanFilter::State fuse(const KalmanFilter::State& p, GaussianFactor::shared_ptr newFactor,
+  KalmanFilter::State fuse(KalmanFilter::State p, GaussianFactor::shared_ptr newFactor,
     const GaussianFactorGraph::Eliminate& function)
   {
     // Create a factor graph
     GaussianFactorGraph factorGraph;
-    factorGraph += p, newFactor;
+    factorGraph.push_back(p);
+    factorGraph.push_back(newFactor);
+//    factorGraph += p, newFactor; // FIXME: bad insert at some point here - fills in a template argument with a shared_ptr where a non-pointer should go
 
     // Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
     return solve(factorGraph, function);