diff --git a/cpp/ChordalBayesNet.cpp b/cpp/ChordalBayesNet.cpp
index 3b0053d52..47e6b8e57 100644
--- a/cpp/ChordalBayesNet.cpp
+++ b/cpp/ChordalBayesNet.cpp
@@ -53,6 +53,7 @@ boost::shared_ptr<FGConfig> ChordalBayesNet::optimize(const boost::shared_ptr<FG
   /** solve each node in turn in topological sort order (parents first)*/
   BOOST_FOREACH(string key, keys) {
     const_iterator cg = nodes.find(key); // get node
+    assert( cg != nodes.end() );
     Vector x = cg->second->solve(*result);                   // Solve it
     result->insert(key,x);   // store result in partial solution
   }
diff --git a/cpp/ConditionalGaussian.h b/cpp/ConditionalGaussian.h
index 84d5e5560..977f4710d 100644
--- a/cpp/ConditionalGaussian.h
+++ b/cpp/ConditionalGaussian.h
@@ -112,7 +112,7 @@ namespace gtsam {
     /**
      * adds a parent
      */    
-    void add(std::string key, Matrix S){ parents_.insert(make_pair(key, S)); }
+    void add(const std::string key, Matrix S){ parents_.insert(make_pair(key, S)); }
 			
     /** equals function */
     bool equals(const ConditionalGaussian &cg) const;