Merge remote-tracking branch 'origin/develop' into fix/isam2

bitbucket-pipelines.yml

@@ -1,9 +1,8 @@
-# This is a sample build configuration for C++ – Make.
+# Built from sample configuration for C++ – Make.
-# Check our guides at https://confluence.atlassian.com/x/5Q4SMw for more examples.
+# Check https://confluence.atlassian.com/x/5Q4SMw for more examples.
-# Only use spaces to indent your .yml configuration.
 # -----
-# You can specify a custom docker image from Docker Hub as your build environment.
+# Our custom docker image from Docker Hub as the build environment.
-image: ilssim/cmake-boost-qt5
+image: dellaert/ubuntu-boost-tbb-eigen3:bionic
 
 pipelines:
   default:
@@ -11,6 +10,6 @@ pipelines:
         script: # Modify the commands below to build your repository.
           - mkdir build
           - cd build
-          - cmake .. 
+          - cmake -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_USE_EIGEN_MKL=OFF .. 
-          - make
+          - make -j2
-          - make check
+          - make -j2 check

docker/ubuntu-boost-tbb-eigen3/Dockerfile

@@ -0,0 +1,18 @@
+# Get the base Ubuntu image from Docker Hub
+FROM ubuntu:bionic
+
+# Update apps on the base image
+RUN apt-get -y update && apt-get install -y
+
+# Install C++
+RUN apt-get -y install build-essential 
+
+# Install boost and cmake
+RUN apt-get -y install libboost-all-dev cmake
+
+# Install TBB
+RUN apt-get -y install libtbb-dev
+
+# Install latest Eigen
+RUN apt-get install -y libeigen3-dev
+

examples/SolverComparer.cpp

@@ -206,7 +206,7 @@ int main(int argc, char *argv[]) {
   }
 
 #ifdef GTSAM_USE_TBB
-  std::auto_ptr<tbb::task_scheduler_init> init;
+  std::unique_ptr<tbb::task_scheduler_init> init;
   if(nThreads > 0) {
     cout << "Using " << nThreads << " threads" << endl;
     init.reset(new tbb::task_scheduler_init(nThreads));

gtsam/3rdparty/CCOLAMD/Source/ccolamd.c

@@ -1560,8 +1560,9 @@ PUBLIC Int CCOLAMD_2	    /* returns TRUE if successful, FALSE otherwise */
     Int *dead_cols ;
     Int set1 ;
     Int set2 ;
+#ifndef NDEBUG
     Int cs ;
-
+#endif
     int ok ;
 
 #ifndef NDEBUG
@@ -1909,8 +1910,10 @@ PUBLIC Int CCOLAMD_2	    /* returns TRUE if successful, FALSE otherwise */
             p [k] = col ;
             ASSERT (A [col] == EMPTY) ;
 
-	    cs = CMEMBER (col) ;
+#ifndef NDEBUG
+	    	cs = CMEMBER (col) ;
             ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
+#endif
 
             A [col] = k ;
             k++ ;
@@ -1926,11 +1929,11 @@ PUBLIC Int CCOLAMD_2	    /* returns TRUE if successful, FALSE otherwise */
             if (A [col] == EMPTY)
             {
                 k = Col [col].shared2.order ;
-		cs = CMEMBER (col) ;
 #ifndef NDEBUG
+				cs = CMEMBER (col) ;
                 dead_cols [cs]-- ;
-#endif
                 ASSERT (k >= cset_start [cs] && k < cset_start [cs+1]) ;
+#endif
                 DEBUG1 (("ccolamd output ordering: k "ID" col "ID
                     " (dense or null col)\n", k, col)) ;
                 p [k] = col ;

gtsam/3rdparty/metis/GKlib/csr.c

@@ -1323,55 +1323,52 @@ void gk_csr_Normalize(gk_csr_t *mat, int what, int norm)
   ssize_t *ptr;
   float *val, sum;
 
-  if (what&GK_CSR_ROW && mat->rowval) {
+  if (what & GK_CSR_ROW && mat->rowval) {
-    n   = mat->nrows;
+    n = mat->nrows;
     ptr = mat->rowptr;
     val = mat->rowval;
 
-    #pragma omp parallel if (ptr[n] > OMPMINOPS) 
+#pragma omp parallel if (ptr[n] > OMPMINOPS)
     {
-      #pragma omp for private(j,sum) schedule(static)
+#pragma omp for private(j, sum) schedule(static)
-      for (i=0; i<n; i++) {
+      for (i = 0; i < n; i++) {
-        for (sum=0.0, j=ptr[i]; j<ptr[i+1]; j++){
+        for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++) {
-  	if (norm == 2)
+          if (norm == 2)
-  	  sum += val[j]*val[j];
+            sum += val[j] * val[j];
-  	else if (norm == 1)
+          else if (norm == 1)
-  	  sum += val[j]; /* assume val[j] > 0 */ 
+            sum += val[j]; /* assume val[j] > 0 */
         }
         if (sum > 0) {
-  	if (norm == 2)
+          if (norm == 2)
-  	  sum=1.0/sqrt(sum); 
+            sum = 1.0 / sqrt(sum);
-  	else if (norm == 1)
+          else if (norm == 1)
-  	  sum=1.0/sum; 
+            sum = 1.0 / sum;
-          for (j=ptr[i]; j<ptr[i+1]; j++)
+          for (j = ptr[i]; j < ptr[i + 1]; j++) val[j] *= sum;
-            val[j] *= sum;
-  	
         }
       }
     }
   }
 
-  if (what&GK_CSR_COL && mat->colval) {
+  if (what & GK_CSR_COL && mat->colval) {
-    n   = mat->ncols;
+    n = mat->ncols;
     ptr = mat->colptr;
     val = mat->colval;
 
-    #pragma omp parallel if (ptr[n] > OMPMINOPS)
+#pragma omp parallel if (ptr[n] > OMPMINOPS)
     {
-    #pragma omp for private(j,sum) schedule(static)
+#pragma omp for private(j, sum) schedule(static)
-      for (i=0; i<n; i++) {
+      for (i = 0; i < n; i++) {
-        for (sum=0.0, j=ptr[i]; j<ptr[i+1]; j++)
+        for (sum = 0.0, j = ptr[i]; j < ptr[i + 1]; j++)
-  	if (norm == 2)
+          if (norm == 2)
-  	  sum += val[j]*val[j];
+            sum += val[j] * val[j];
-  	else if (norm == 1)
+          else if (norm == 1)
-  	  sum += val[j]; 
+            sum += val[j];
         if (sum > 0) {
-  	if (norm == 2)
+          if (norm == 2)
-  	  sum=1.0/sqrt(sum); 
+            sum = 1.0 / sqrt(sum);
-  	else if (norm == 1)
+          else if (norm == 1)
-  	  sum=1.0/sum; 
+            sum = 1.0 / sum;
-          for (j=ptr[i]; j<ptr[i+1]; j++)
+          for (j = ptr[i]; j < ptr[i + 1]; j++) val[j] *= sum;
-            val[j] *= sum;
         }
       }
     }

gtsam/3rdparty/metis/GKlib/getopt.c

@@ -341,10 +341,10 @@ static int gk_getopt_internal(int argc, char **argv, char *optstring,
 
   if (gk_optind == 0 || !gk_getopt_initialized) {
     if (gk_optind == 0)
-      gk_optind = 1;	/* Don't scan ARGV[0], the program name.  */
+      gk_optind = 1; /* Don't scan ARGV[0], the program name.  */
-      optstring = gk_getopt_initialize (argc, argv, optstring);
+    optstring = gk_getopt_initialize(argc, argv, optstring);
-      gk_getopt_initialized = 1;
+    gk_getopt_initialized = 1;
-    }
+  }
 
   /* Test whether ARGV[gk_optind] points to a non-option argument.
      Either it does not have option syntax, or there is an environment flag
@@ -670,37 +670,38 @@ static int gk_getopt_internal(int argc, char **argv, char *optstring,
 
     if (temp[1] == ':') {
       if (temp[2] == ':') {
-	/* This is an option that accepts an argument optionally.  */
+        /* This is an option that accepts an argument optionally.  */
-	if (*nextchar != '\0') {
+        if (*nextchar != '\0') {
-  	  gk_optarg = nextchar;
+          gk_optarg = nextchar;
-	  gk_optind++;
+          gk_optind++;
-	}
+        } else {
-	else
+          gk_optarg = NULL;
-	  gk_optarg = NULL;
+        }
-	nextchar = NULL;
+        nextchar = NULL;
-      }
+      } else {
-      else {
+        /* This is an option that requires an argument.  */
-	/* This is an option that requires an argument.  */
+        if (*nextchar != '\0') {
-	if (*nextchar != '\0') {
+          gk_optarg = nextchar;
-	  gk_optarg = nextchar;
+          /* If we end this ARGV-element by taking the rest as an arg, we must
-	  /* If we end this ARGV-element by taking the rest as an arg, we must advance to the next element now.  */
+           * advance to the next element now.  */
-	  gk_optind++;
+          gk_optind++;
-	}
+        } else if (gk_optind == argc) {
-	else if (gk_optind == argc) {
+          if (print_errors) {
-	  if (print_errors) {
+            /* 1003.2 specifies the format of this message.  */
-	    /* 1003.2 specifies the format of this message.  */
+            fprintf(stderr, "%s: option requires an argument -- %c\n", argv[0],
-	    fprintf(stderr, "%s: option requires an argument -- %c\n", argv[0], c);
+                    c);
-	  }
+          }
-	  gk_optopt = c;
+          gk_optopt = c;
-	  if (optstring[0] == ':')
+          if (optstring[0] == ':')
-	    c = ':';
+            c = ':';
-	  else
+          else
-	    c = '?';
+            c = '?';
-	}
+        } else {
-	else
+          /* We already incremented `gk_optind' once; increment it again when
-	  /* We already incremented `gk_optind' once; increment it again when taking next ARGV-elt as argument.  */
+           * taking next ARGV-elt as argument.  */
-	  gk_optarg = argv[gk_optind++];
+          gk_optarg = argv[gk_optind++];
-	  nextchar = NULL;
+        }
+        nextchar = NULL;
       }
     }
     return c;

gtsam/base/cholesky.cpp

@@ -111,8 +111,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) {
     return true;
 
   assert(ABC.cols() == ABC.rows());
-  const Eigen::DenseIndex n = ABC.rows() - topleft;
+  assert(ABC.rows() >= topleft);
-  assert(n >= 0 && nFrontal <= size_t(n));
+  const size_t n = static_cast<size_t>(ABC.rows() - topleft);
+  assert(nFrontal <= size_t(n));
 
   // Create views on blocks
   auto A = ABC.block(topleft, topleft, nFrontal, nFrontal);

gtsam/geometry/Unit3.cpp

@@ -21,7 +21,7 @@
 
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/config.h> // for GTSAM_USE_TBB
+#include <gtsam/config.h>  // for GTSAM_USE_TBB
 
 #ifdef __clang__
 #  pragma clang diagnostic push
@@ -36,13 +36,14 @@
 #include <iostream>
 #include <limits>
 #include <cmath>
+#include <vector>
 
 using namespace std;
 
 namespace gtsam {
 
 /* ************************************************************************* */
-Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
+Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2, 3> H) {
   // 3*3 Derivative of representation with respect to point is 3*3:
   Matrix3 D_p_point;
   Unit3 direction;
@@ -54,7 +55,7 @@ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
 
 /* ************************************************************************* */
 Unit3 Unit3::Random(boost::mt19937 & rng) {
-  // TODO allow any engine without including all of boost :-(
+  // TODO(dellaert): allow any engine without including all of boost :-(
   boost::uniform_on_sphere<double> randomDirection(3);
   // This variate_generator object is required for versions of boost somewhere
   // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
@@ -161,7 +162,8 @@ Matrix3 Unit3::skew() const {
 }
 
 /* ************************************************************************* */
-double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,2> H_q) const {
+double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
+                  OptionalJacobian<1, 2> H_q) const {
   // Get the unit vectors of each, and the derivative.
   Matrix32 H_pn_p;
   Point3 pn = point3(H_p ? &H_pn_p : nullptr);
@@ -185,7 +187,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
+Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
   const Vector2 xi = basis().transpose() * q.p_;
   if (H_q) {
@@ -195,7 +197,8 @@ Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const {
 }
 
 /* ************************************************************************* */
-Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJacobian<2, 2> H_q) const {
+Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
+                           OptionalJacobian<2, 2> H_q) const {
   // Get the point3 of this, and the derivative.
   Matrix32 H_qn_q;
   const Point3 qn = q.point3(H_q ? &H_qn_q : nullptr);
@@ -230,7 +233,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
 }
 
 /* ************************************************************************* */
-double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
+double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
   Matrix2 H_xi_q;
   const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
   const double theta = xi.norm();
@@ -277,4 +280,4 @@ Vector2 Unit3::localCoordinates(const Unit3& other) const {
 }
 /* ************************************************************************* */
 
-}
+}  // namespace gtsam

gtsam/geometry/tests/testPoint2.cpp

@@ -100,12 +100,12 @@ TEST( Point2, expmap) {
 
 /* ************************************************************************* */
 TEST( Point2, arithmetic) {
-  EXPECT(assert_equal<Point2>( Point2(-5,-6), -Point2(5,6) ));
+  EXPECT(assert_equal<Point2>(Point2(-5, -6), -Point2(5, 6)));
-  EXPECT(assert_equal<Point2>( Point2(5,6), Point2(4,5)+Point2(1,1)));
+  EXPECT(assert_equal<Point2>(Point2(5, 6), Point2(4, 5) + Point2(1, 1)));
-  EXPECT(assert_equal<Point2>( Point2(3,4), Point2(4,5)-Point2(1,1) ));
+  EXPECT(assert_equal<Point2>(Point2(3, 4), Point2(4, 5) - Point2(1, 1)));
-  EXPECT(assert_equal<Point2>( Point2(8,6), Point2(4,3)*2));
+  EXPECT(assert_equal<Point2>(Point2(8, 6), Point2(4, 3) * 2));
-  EXPECT(assert_equal<Point2>( Point2(4,6), 2*Point2(2,3)));
+  EXPECT(assert_equal<Point2>(Point2(4, 6), 2.0 * Point2(2, 3)));
-  EXPECT(assert_equal<Point2>( Point2(2,3), Point2(4,6)/2));
+  EXPECT(assert_equal<Point2>(Point2(2, 3), Point2(4, 6) / 2));
 }
 
 /* ************************************************************************* */

gtsam/inference/BayesTree-inst.h

@@ -83,26 +83,28 @@ namespace gtsam {
     std::string parent = out.str();
     parent += "[label=\"";
 
-    for(Key index: clique->conditional_->frontals()) {
+    for (Key index : clique->conditional_->frontals()) {
-      if(!first) parent += ","; first = false;
+      if (!first) parent += ",";
+      first = false;
       parent += indexFormatter(index);
     }
 
-    if(clique->parent()){
+    if (clique->parent()) {
       parent += " : ";
       s << parentnum << "->" << num << "\n";
     }
 
     first = true;
-    for(Key sep: clique->conditional_->parents()) {
+    for (Key sep : clique->conditional_->parents()) {
-      if(!first) parent += ","; first = false;
+      if (!first) parent += ",";
+      first = false;
       parent += indexFormatter(sep);
     }
     parent += "\"];\n";
     s << parent;
     parentnum = num;
 
-    for(sharedClique c: clique->children) {
+    for (sharedClique c : clique->children) {
       num++;
       saveGraph(s, c, indexFormatter, parentnum);
     }

wrap/FileWriter.cpp

@@ -29,7 +29,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const {
   bool file_exists = true;
   try {
     existing_contents = file_contents(filename_.c_str(), add_header);
-  } catch (CantOpenFile) {
+  } catch (const CantOpenFile& e) {
     file_exists = false;
   }