Merge branch 'develop' into refactor/ExpressionTests

.github/scripts/boost.sh

@@ -2,7 +2,7 @@
 BOOST_FOLDER=boost_${BOOST_VERSION//./_}
 
 # Download Boost
-wget https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
+wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
 
 # Unzip
 tar -zxf ${BOOST_FOLDER}.tar.gz

.github/workflows/build-linux.yml

@@ -57,7 +57,7 @@ jobs:
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
             # This key is not in the keystore by default for Ubuntu so we need to add it.
             LLVM_KEY=15CF4D18AF4F7421
-            gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi

.github/workflows/build-python.yml

@@ -19,22 +19,20 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-          # ubuntu-18.04-gcc-5,  #TODO Enable once the Python wrapper is optimized for memory
+          ubuntu-18.04-gcc-5,
           ubuntu-18.04-gcc-9,
           ubuntu-18.04-clang-9,
           macOS-10.15-xcode-11.3.1,
-          # ubuntu-18.04-gcc-5-tbb,
+          ubuntu-18.04-gcc-5-tbb,
         ]
 
-        #TODO update wrapper to prevent OOM
-        # build_type: [Debug, Release]
-        build_type: [Release]
+        build_type: [Debug, Release]
         python_version: [3]
         include:
-          # - name: ubuntu-18.04-gcc-5
-          #   os: ubuntu-18.04
-          #   compiler: gcc
-          #   version: "5"
+          - name: ubuntu-18.04-gcc-5
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "5"
 
           - name: ubuntu-18.04-gcc-9
             os: ubuntu-18.04
@@ -46,7 +44,7 @@ jobs:
             compiler: clang
             version: "9"
 
-          #NOTE temporarily added this as it is a required check.
+          # NOTE temporarily added this as it is a required check.
           - name: ubuntu-18.04-clang-9
             os: ubuntu-18.04
             compiler: clang
@@ -59,11 +57,11 @@ jobs:
             compiler: xcode
             version: "11.3.1"
 
-          # - name: ubuntu-18.04-gcc-5-tbb
-          #   os: ubuntu-18.04
-          #   compiler: gcc
-          #   version: "5"
-          #   flag: tbb
+          - name: ubuntu-18.04-gcc-5-tbb
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "5"
+            flag: tbb
 
     steps:
       - name: Checkout
@@ -77,7 +75,7 @@ jobs:
             # 15CF4D18AF4F7421 is the GPG key for the LLVM apt repository
             # This key is not in the keystore by default for Ubuntu so we need to add it.
             LLVM_KEY=15CF4D18AF4F7421
-            gpg --keyserver ipv4.pool.sks-keyservers.net --recv-key $LLVM_KEY || gpg --keyserver ha.pool.sks-keyservers.net --recv-key $LLVM_KEY
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export $LLVM_KEY | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi

.github/workflows/build-special.yml

@@ -64,7 +64,7 @@ jobs:
         run: |
           # LLVM 9 is not in Bionic's repositories so we add the official LLVM repository.
           if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ]; then
-            gpg --keyserver pool.sks-keyservers.net --recv-key 15CF4D18AF4F7421
+            gpg --keyserver keyserver.ubuntu.com --recv-key $LLVM_KEY || gpg --keyserver hkp://keyserver.ubuntu.com:80 --recv-key $LLVM_KEY
             gpg -a --export 15CF4D18AF4F7421 | sudo apt-key add -
             sudo add-apt-repository "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-9 main"
           fi

INSTALL.md

@@ -20,9 +20,10 @@ $ make install
     Optional dependent libraries:
      - If TBB is installed and detectable by CMake GTSAM will use it automatically.
        Ensure that CMake prints "Use Intel TBB : Yes".  To disable the use of TBB,
-       disable the CMake flag GTSAM_WITH_TBB (enabled by default).  On Ubuntu, TBB
-       may be installed from the Ubuntu repositories, and for other platforms it
-       may be downloaded from https://www.threadingbuildingblocks.org/
+       disable the CMake flag `GTSAM_WITH_TBB` (enabled by default) by providing
+       the argument `-DGTSAM_WITH_TBB=OFF` to `cmake`.  On Ubuntu, TBB may be
+       installed from the Ubuntu repositories, and for other platforms it may be
+       downloaded from https://www.threadingbuildingblocks.org/
      - GTSAM may be configured to use MKL by toggling `GTSAM_WITH_EIGEN_MKL` and
        `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually
        achieved with MKL disabled. We therefore advise you to benchmark your problem 

examples/CombinedImuFactorsExample.cpp

@@ -23,7 +23,8 @@
  *  A row starting with "i" is the first initial position formatted with
  *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
  *  A row starting with "0" is an imu measurement
- *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  (body frame - Forward, Right, Down)
+ *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
  *  A row starting with "1" is a gps correction formatted with
  *  N, E, D, qX, qY, qZ, qW
  * Note that for GPS correction, we're only using the position not the

examples/CreateSFMExampleData.cpp

@@ -15,8 +15,8 @@
  * @author  Frank Dellaert
  */
 
-#include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/slam/dataset.h>
 
 #include <boost/assign/std/vector.hpp>
 
@@ -26,22 +26,16 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 
-void createExampleBALFile(const string& filename, const vector<Point3>& P,
-    const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
-        Cal3Bundler()) {
-
+void createExampleBALFile(const string& filename, const vector<Point3>& points,
+                          const Pose3& pose1, const Pose3& pose2,
+                          const Cal3Bundler& K = Cal3Bundler()) {
   // Class that will gather all data
   SfmData data;
-
-  // Create two cameras
-  Rot3 aRb = Rot3::Yaw(M_PI_2);
-  Point3 aTb(0.1, 0, 0);
-  Pose3 identity, aPb(aRb, aTb);
+  // Create two cameras and add them to data
   data.cameras.push_back(SfmCamera(pose1, K));
   data.cameras.push_back(SfmCamera(pose2, K));
 
-  for(const Point3& p: P) {
-
+  for (const Point3& p : points) {
     // Create the track
     SfmTrack track;
     track.p = p;
@@ -51,7 +45,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 
     // Project points in both cameras
     for (size_t i = 0; i < 2; i++)
-    track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
+      track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
 
     // Add track to data
     data.tracks.push_back(track);
@@ -63,49 +57,66 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
 /* ************************************************************************* */
 
 void create5PointExample1() {
-
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need at least 5 points
-  vector<Point3> P;
-  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
-  Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
+  vector<Point3> points = {
+      {0, 0, 1}, {-0.1, 0, 1}, {0.1, 0, 1}, {0, 0.5, 0.5}, {0, -0.5, 0.5}};
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample1.txt";
-  createExampleBALFile(filename, P, pose1, pose2);
+  const string filename = "../../examples/Data/5pointExample1.txt";
+  createExampleBALFile(filename, points, pose1, pose2);
 }
 
 /* ************************************************************************* */
 
 void create5PointExample2() {
-
   // Create two cameras poses
   Rot3 aRb = Rot3::Yaw(M_PI_2);
   Point3 aTb(10, 0, 0);
   Pose3 pose1, pose2(aRb, aTb);
 
   // Create test data, we need at least 5 points
-  vector<Point3> P;
-  P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
-  Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
+  vector<Point3> points = {{0, 0, 100},  {-10, 0, 100}, {10, 0, 100},  //
+                           {0, 50, 50},  {0, -50, 50},  {-20, 0, 80},  //
+                           {20, -50, 80}};
 
   // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample2.txt";
+  const string filename = "../../examples/Data/5pointExample2.txt";
   Cal3Bundler K(500, 0, 0);
-  createExampleBALFile(filename, P, pose1, pose2,K);
+  createExampleBALFile(filename, points, pose1, pose2, K);
 }
 
 /* ************************************************************************* */
 
+void create18PointExample1() {
+  // Create two cameras poses
+  Rot3 aRb = Rot3::Yaw(M_PI_2);
+  Point3 aTb(0.1, 0, 0);
+  Pose3 pose1, pose2(aRb, aTb);
+
+  // Create test data, we need 15 points
+  vector<Point3> points = {
+      {0, 0, 1},         {-0.1, 0, 1},      {0.1, 0, 1},       //
+      {0, 0.5, 0.5},     {0, -0.5, 0.5},    {-1, -0.5, 2},     //
+      {-1, 0.5, 2},      {0.25, -0.5, 1.5}, {0.25, 0.5, 1.5},  //
+      {-0.1, -0.5, 0.5}, {0.1, -0.5, 1},    {0.1, 0.5, 1},     //
+      {-0.1, 0, 0.5},    {-0.1, 0.5, 0.5},  {0, 0, 0.5},       //
+      {0.1, -0.5, 0.5},  {0.1, 0, 0.5},     {0.1, 0.5, 0.5}};
+
+  // Assumes example is run in ${GTSAM_TOP}/build/examples
+  const string filename = "../../examples/Data/18pointExample1.txt";
+  createExampleBALFile(filename, points, pose1, pose2);
+}
+
 int main(int argc, char* argv[]) {
   create5PointExample1();
   create5PointExample2();
+  create18PointExample1();
   return 0;
 }
 
 /* ************************************************************************* */
-

examples/Data/18pointExample1.txt

@@ -0,0 +1,131 @@
+2 18 36
+
+0 0 0 -0
+1 0 -6.123233995736766344e-18 -0.10000000000000000555
+0 1 -0.10000000000000000555 -0
+1 1 -1.2246467991473532688e-17 -0.2000000000000000111
+0 2 0.10000000000000000555 -0
+1 2 0 -0
+0 3 0 -1
+1 3 1 -0.20000000000000006661
+0 4 0 1
+1 4 -1 -0.19999999999999995559
+0 5 -0.5 0.25
+1 5 -0.25000000000000005551 -0.55000000000000004441
+0 6 -0.5 -0.25
+1 6 0.24999999999999997224 -0.55000000000000004441
+0 7 0.16666666666666665741 0.33333333333333331483
+1 7 -0.33333333333333331483 0.10000000000000000555
+0 8 0.16666666666666665741 -0.33333333333333331483
+1 8 0.33333333333333331483 0.099999999999999977796
+0 9 -0.2000000000000000111 1
+1 9 -1 -0.39999999999999996669
+0 10 0.10000000000000000555 0.5
+1 10 -0.5 3.0616169978683830179e-17
+0 11 0.10000000000000000555 -0.5
+1 11 0.5 -3.0616169978683830179e-17
+0 12 -0.2000000000000000111 -0
+1 12 -2.4492935982947065376e-17 -0.4000000000000000222
+0 13 -0.2000000000000000111 -1
+1 13 1 -0.40000000000000007772
+0 14 0 -0
+1 14 -1.2246467991473532688e-17 -0.2000000000000000111
+0 15 0.2000000000000000111 1
+1 15 -1 6.1232339957367660359e-17
+0 16 0.2000000000000000111 -0
+1 16 0 -0
+0 17 0.2000000000000000111 -1
+1 17 1 -6.1232339957367660359e-17
+
+3.141592653589793116
+                   0
+                   0
+-0
+0
+0
+1
+0
+0
+
+2.2214414690791830509
+2.2214414690791826068
+                    0
+-6.123233995736766344e-18
+-0.10000000000000000555
+0
+1
+0
+0
+
+0
+0
+1
+
+-0.10000000000000000555
+0
+1
+
+0.10000000000000000555
+0
+1
+
+0
+0.5
+0.5
+
+0
+-0.5
+0.5
+
+-1
+-0.5
+2
+
+-1
+0.5
+2
+
+0.25
+-0.5
+1.5
+
+0.25
+0.5
+1.5
+
+-0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+-0.5
+1
+
+0.10000000000000000555
+0.5
+1
+
+-0.10000000000000000555
+0
+0.5
+
+-0.10000000000000000555
+0.5
+0.5
+
+0
+0
+0.5
+
+0.10000000000000000555
+-0.5
+0.5
+
+0.10000000000000000555
+0
+0.5
+
+0.10000000000000000555
+0.5
+0.5
+

examples/ImuFactorsExample.cpp

@@ -25,7 +25,8 @@
  *  A row starting with "i" is the first initial position formatted with
  *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
  *  A row starting with "0" is an imu measurement
- *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  (body frame - Forward, Right, Down)
+ *  linAccX, linAccY, linAccZ, angVelX, angVelY, angVelX
  *  A row starting with "1" is a gps correction formatted with
  *  N, E, D, qX, qY, qZ, qW
  * Note that for GPS correction, we're only using the position not the