From 67f3109e755532ca0210b27eac3f7967ebf17c9f Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 1 Jul 2013 20:19:36 +0000
Subject: [PATCH] Fixed warnings in preexisting code

---
 gtsam/base/Vector.cpp                         |  2 +-
 gtsam/inference/inference.cpp                 |  4 +-
 gtsam/linear/GaussianFactorGraph.cpp          | 43 +++++++++++++++++++
 .../nonlinear/LevenbergMarquardtOptimizer.cpp |  1 +
 4 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index 7a71eb125..243e1e990 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -387,7 +387,7 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
 // Slow version with error checking
 pair<Vector, double>
 weightedPseudoinverse(const Vector& a, const Vector& weights) {
-  int m = weights.size();
+  DenseIndex m = weights.size();
   if (a.size() != m)
     throw invalid_argument("a and weights have different sizes!");
   Vector pseudo(m);
diff --git a/gtsam/inference/inference.cpp b/gtsam/inference/inference.cpp
index 2704e8c26..8b8f98139 100644
--- a/gtsam/inference/inference.cpp
+++ b/gtsam/inference/inference.cpp
@@ -38,7 +38,7 @@ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, s
   gttic(Prepare);
   size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
   // Convert to compressed column major format colamd wants it in (== MATLAB format!)
-  int Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
+  size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
   vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
   vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
 
@@ -78,7 +78,7 @@ Permutation::shared_ptr PermutationCOLAMD_(const VariableIndex& variableIndex, s
   /* returns (1) if successful, (0) otherwise*/
   if(nVars > 0) {
     gttic(ccolamd);
-    int rv = ccolamd((int)nFactors, nVars, Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
+    int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
     if(rv != 1)
       throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
   }
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 883542144..3f648042d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -192,6 +192,49 @@ namespace gtsam {
       augmented.col(augmented.cols()-1));
   }
 
+  // Helper functions for Combine
+  static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
+#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
+    vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
+#else
+    vector<size_t> varDims(variableSlots.size());
+#endif
+    size_t m = 0;
+    size_t n = 0;
+    {
+      Index jointVarpos = 0;
+      BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
+
+        assert(slots.second.size() == factors.size());
+
+        Index sourceFactorI = 0;
+        BOOST_FOREACH(const Index sourceVarpos, slots.second) {
+          if(sourceVarpos < numeric_limits<Index>::max()) {
+            const JacobianFactor& sourceFactor = *factors[sourceFactorI];
+            size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
+#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
+            if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
+              varDims[jointVarpos] = vardim;
+              n += vardim;
+            } else
+              assert(varDims[jointVarpos] == vardim);
+#else
+            varDims[jointVarpos] = vardim;
+            n += vardim;
+            break;
+#endif
+          }
+          ++ sourceFactorI;
+        }
+        ++ jointVarpos;
+      }
+      BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
+        m += factor->rows();
+      }
+    }
+    return boost::make_tuple(varDims, m, n);
+  }
+
   /* ************************************************************************* */
   JacobianFactor::shared_ptr CombineJacobians(
       const FactorGraph<JacobianFactor>& factors,
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 8505834b5..48ed5d1c9 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -143,6 +143,7 @@ void LevenbergMarquardtOptimizer::iterate() {
         }
       }
     } catch(const IndeterminantLinearSystemException& e) {
+      (void) e; // Prevent unused variable warning
       if(lmVerbosity >= LevenbergMarquardtParams::LAMBDA)
         cout << "Negative matrix, increasing lambda" << endl;
       // Either we're not cautious, or the same lambda was worse than the current error.