Add ubuntu ci

.github/workflows/build-test-linux.yml

@@ -0,0 +1,140 @@
+name: Linux CI
+
+on: [push, pull_request]
+
+jobs:
+  build:
+    name: ${{ matrix.name }} ${{ matrix.build_type }}
+    runs-on: ${{ matrix.os }}
+
+    env:
+      CTEST_OUTPUT_ON_FAILURE: ON
+      CTEST_PARALLEL_LEVEL: 2
+      CMAKE_BUILD_TYPE: ${{ matrix.build_type }}
+      BOOST_VERSION: 1.67.0
+
+    strategy:
+      fail-fast: false
+      matrix:
+        name: [
+            ubuntu-18.04-gcc-5,
+            ubuntu-18.04-gcc-9,
+            ubuntu-18.04-clang-9,
+            ubuntu-20.04-gcc-7,
+            ubuntu-20.04-gcc-9,
+            ubuntu-20.04-clang-9,
+        ]
+
+        build_type: [Debug, Release]
+        include:
+          - name: ubuntu-18.04-gcc-5
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "5"
+
+          - name: ubuntu-18.04-gcc-9
+            os: ubuntu-18.04
+            compiler: gcc
+            version: "9"
+
+          - name: ubuntu-18.04-clang-9
+            os: ubuntu-18.04
+            compiler: clang
+            version: "9"
+
+          - name: ubuntu-20.04-gcc-7
+            os: ubuntu-20.04
+            compiler: gcc
+            version: "7"
+
+          - name: ubuntu-20.04-gcc-9
+            os: ubuntu-20.04
+            compiler: gcc
+            version: "9"
+
+          - name: ubuntu-20.04-clang-9
+            os: ubuntu-20.04
+            compiler: clang
+            version: "9"
+
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v2
+
+      - name: Install Eigen 3.3.9
+        run: |
+          cd ${{runner.workspace}} # cd to runner home
+          git clone --depth 1 --branch 3.3.9 https://gitlab.com/libeigen/eigen eigen3 # clone Eigen
+          cd eigen3 && mkdir build && cd build # move into build dir for eigen3
+          cmake ..
+          sudo make -j$(nproc) install
+      - name: Install Python 3.9
+        run: |
+          if ["${{matrix.os}}"="ubuntu-18.04"]; then
+            sudo apt-get -y install libpython-dev python-numpy
+          else # Ubuntu 20.04
+            sudo apt-get -y install libpython3-dev python-numpy
+          fi
+      - name: Install build tools
+        run: |
+          # LLVM (clang) 9 is not in Bionic's repositories so we add the official LLVM repository.
+          if [ "${{ matrix.compiler }}" = "clang" ] && [ "${{ matrix.version }}" = "9" ] && [ "${{matrix.os}}"="ubuntu-18.04" ]; then
+            # (ipv4|ha).pool.sks-keyservers.net is the SKS GPG global keyserver pool
+            # ipv4 avoids potential timeouts because of crappy IPv6 infrastructure
+            # 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 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
+          sudo apt-get -y update
+          sudo apt-get -y install cmake build-essential pkg-config libicu-dev
+          if [ "${{ matrix.compiler }}" = "gcc" ]; then
+            sudo apt-get install -y g++-${{ matrix.version }} g++-${{ matrix.version }}-multilib
+            echo "CC=gcc-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=g++-${{ matrix.version }}" >> $GITHUB_ENV
+          else
+            sudo apt-get install -y clang-${{ matrix.version }} g++-multilib
+            echo "CC=clang-${{ matrix.version }}" >> $GITHUB_ENV
+            echo "CXX=clang++-${{ matrix.version }}" >> $GITHUB_ENV
+          fi
+      - name: Install Boost
+        run: |
+          BOOST_FOLDER=boost_${BOOST_VERSION//./_}
+          wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz
+          tar -zxf ${BOOST_FOLDER}.tar.gz
+          cd ${BOOST_FOLDER}/
+          ./bootstrap.sh --with-libraries=serialization,filesystem,thread,system,atomic,date_time,timer,chrono,program_options,regex
+          sudo ./b2 -j$(nproc) install
+      - name: Install GTSAM 4.0.3
+        run: |
+          cd ${{runner.workspace}} # cd to runner home
+          git clone --depth 1 --branch 4.0.3 https://github.com/borglab/gtsam.git gtsam # clone latest from dev branch. switch to GTSAM 4.1 once it's tagged.
+          cd gtsam && mkdir build && cd build # move into build dir for gtsam
+          cmake -DCMAKE_BUILD_TYPE=Release -DGTSAM_WITH_EIGEN_MKL=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_BUILD_TIMING_ALWAYS=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_UNSTABLE=ON ..
+          sudo make -j$(nproc) install
+      - name: Install hdf5
+        run: sudo apt-get -y install libhdf5-serial-dev
+
+      - name: Install HighFive
+        run: |
+          cd ${{runner.workspace}} # cd to runner home
+          git clone --depth 1 --branch master https://github.com/BlueBrain/HighFive HighFive # clone HighFive
+          cd HighFive && mkdir build && cd build # move into build dir for HighFive
+          cmake -DHIGHFIVE_USE_BOOST=OFF ..
+          sudo make -j$(nproc) install
+      - name: Install imuDataUtils
+        run: |
+          cd ${{runner.workspace}} # cd to runner home
+          git clone --depth 1 --branch master https://github.com/tmcg0/imuDataUtils imuDataUtils # clone imuDataUtils
+          cd imuDataUtils && mkdir build && cd build # move into build dir for imuDataUtils
+          cmake ..
+          sudo make -j$(nproc) install
+      - name: Build and test bioslam
+        run: |
+          cd ${{github.workspace}} # cd to repo root
+          mkdir build && cd build # move into bioslam build dir
+          cmake -DBIOSLAM_BUILD_MATLAB_WRAPPER=OFF ..
+          sudo make -j$(nproc) install
+          make -j$(nproc) test

CMakeLists.txt

@@ -28,7 +28,7 @@ set(CMAKE_CXX_STANDARD 14) # C++ version
 option(BIOSLAM_BUILD_WITH_MARCH_NATIVE "Build with -march=native (this should match the gtsam build option GTSAM_BUILD_WITH_MARCH_NATIVE)" ON)
 option(BIOSLAM_BUILD_TESTS "Should I build these tests?" ON)
 option(GTSAM_USE_MKL "Does GTSAM require MKL?" OFF)
-option(BIOSLAM_BUILD_MATLAB_WRAPPER "Build the MATLAB interface for bioslam?" ON)
+option(BIOSLAM_BUILD_MATLAB_WRAPPER "Build the MATLAB interface for bioslam?" OFF)
 option(BIOSLAM_BUILD_EXAMPLES "Build examples?" ON)
 option(BIOSLAM_USE_TBB "Include Intel's TBB library?" ON) # if GTSAM uses TBB, you need to set this to 'ON'
 ### ------------------------ ###

README.md

@@ -1,4 +1,4 @@
-# Bioslam: IMU-Based Human Skeletal Pose Estimation
+# Bioslam: IMU-Based Human Skeletal Pose Estimation ![Built, Test CI](https://github.com/tmcg0/bioslam/actions/workflows/build-test-linux.yml/badge.svg)
 
 **Author:** [Tim McGrath](https://www.researchgate.net/profile/Tim_Mcgrath9)
 
@@ -35,10 +35,20 @@ The optimization backend ([GTSAM 4.0+](https://github.com/borglab/gtsam)) and IM
 
 # Installation
 
+### Supported systems
+Bioslam is tested on the following systems
+
+- Ubuntu 18.04 with GCC 5, GCC 9, and Clang 9
+- Ubuntu 20.04 with GCC 7, GCC 9, and Clang 9
+
+using Boost 1.67, GTSAM 4.0.3, and Eigen 3.3.9.
+
+On other systems, a bioslam-installed Ubuntu image can be spun up in a Docker container using the provided [Dockerfile](docker/Dockerfile).
+
 ## Required Dependencies
 
 * CMake (>= 3.17, _working through older versions now. Can confirm 3.10.2 doesn't work_)
-* boost (>= 1.58)
+* boost (>= 1.65)
   * Ubuntu: `sudo apt-get install libboost-all-dev`
 * [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page)
   * `git clone https://github.com/eigenteam/eigen-git-mirror`

include/factors/Pose3Priors.h

@@ -71,6 +71,8 @@ namespace bioslam {
 
         static gtsam::Vector1 errorModel(const gtsam::Pose3& pose,const gtsam::Vector3& refVecLocal, const gtsam::Vector3& refVecNav, const gtsam::Vector3& upVecNav, double expectedAng, boost::optional<gtsam::Matrix16&> Hx = boost::none);
         static double compassAngle(const gtsam::Rot3& r, const gtsam::Vector3& refVecLocal, const gtsam::Vector3& refVecNav, const gtsam::Vector3& upVecNav, boost::optional<gtsam::Matrix13&> Hr = boost::none);
+        static double compassAngleFromVertical(const gtsam::Rot3& r, const gtsam::Vector3& refVecLocal, const gtsam::Vector3& upVecNav); // returns how far compass is from being vertical (radians)
+        static bool isCompassVertical(const gtsam::Rot3& r, const gtsam::Vector3& refVecLocal, const gtsam::Vector3& upVecNav, double verticalAngTolRadians=10.0*M_PI/180.0); // returns true/false decision on if compass is held vertically
 
         gtsam::Vector evaluateError(const gtsam::Pose3& pose, boost::optional<gtsam::Matrix &> H1 = boost::none) const;
 

src/factors/Pose3Priors.cpp

@@ -31,26 +31,36 @@ namespace bioslam{
         return e;
     }
 
-
     // --- compass prior factor --- //
     double Pose3CompassPrior::compassAngle(const gtsam::Rot3& r, const gtsam::Vector3& refVecLocal, const gtsam::Vector3& refVecNav, const gtsam::Vector3& upVecNav, boost::optional<gtsam::Matrix13&> Hr){
         // compute the compass-style angle from a Rot3 and optionally return derivatives
         // call local compass vector l
         gtsam::Matrix33 dlN_dr;
         gtsam::Vector3 lN=r.rotate(refVecLocal,dlN_dr, boost::none); // rotate l into nav frame
         // debug check: make sure compass isn't vertical. this represents an ambiguity (gimbal lock) of the compass.
-        double vertAngTol=10.0*M_PI/180.0; // this many degrees within 0 or 180 throws error in debug mode
-        if(mathutils::unsignedAngle(lN, upVecNav) < vertAngTol || mathutils::unsignedAngle(lN, upVecNav) > (M_PI - vertAngTol)){
-            std::cerr << "WARNING: compass vector is near up/down vertical (ang=" << mathutils::unsignedAngle(lN, upVecNav) << ")" << std::endl;
+        if(Pose3CompassPrior::isCompassVertical(r,refVecLocal,upVecNav)){
+            std::cout<< "WARNING: compass vector is near vertical (ang=" << mathutils::unsignedAngle(lN, upVecNav) << ")" << std::endl;
         }
         gtsam::Matrix33 dlNH_dlN;
-        gtsam::Vector3 dlNH=mathutils::projVecIntoPlane(lN, upVecNav, dlNH_dlN, boost::none); // project lN into horizontal plane
+        gtsam::Vector3 lNH=mathutils::projVecIntoPlane(lN, upVecNav, dlNH_dlN, boost::none); // project lN into nav horizontal plane
         gtsam::Matrix13 dang_dlNH;
-        double ang=mathutils::signedAngle(dlNH, refVecNav, upVecNav, dang_dlNH, boost::none); // compass angle, radians
+        double ang=mathutils::signedAngle(lNH, refVecNav, upVecNav, dang_dlNH, boost::none); // compass angle, radians
         if(Hr){*Hr=dang_dlNH*dlNH_dlN*dlN_dr;} // dang/dr
         return ang;
     }
 
+    double Pose3CompassPrior::compassAngleFromVertical(const gtsam::Rot3& r, const gtsam::Vector3& refVecLocal, const gtsam::Vector3& upVecNav){
+        // returns how far compass is from being vertical (radians). 0 implies compass is pointed exactly up, pi implies compass is pointed down.
+        gtsam::Vector3 lN=r.rotate(refVecLocal, boost::none, boost::none); // rotate compass vector l into nav frame
+        return mathutils::unsignedAngle(lN, upVecNav);
+    }
+
+    bool Pose3CompassPrior::isCompassVertical(const gtsam::Rot3& r, const gtsam::Vector3& refVecLocal, const gtsam::Vector3& upVecNav, double verticalAngTolRadians){
+        // returns true/false decision on if compass is held vertically (i.e., up or down)
+        double compassVertAngle=Pose3CompassPrior::compassAngleFromVertical(r,refVecLocal,upVecNav);
+        return (abs(compassVertAngle)<verticalAngTolRadians || abs(compassVertAngle)>(M_PI-verticalAngTolRadians));
+    }
+
     gtsam::Vector1 Pose3CompassPrior::errorModel(const gtsam::Pose3& pose,const gtsam::Vector3& refVecLocal, const gtsam::Vector3& refVecNav, const gtsam::Vector3& upVecNav, double expectedAng, boost::optional<gtsam::Matrix16&> Hx){
         // implements error model for this factor
         gtsam::Matrix36 dr_dx;

test/unit/testHingeJointConstraintNormErrorEstAngVel.cpp

@@ -222,7 +222,7 @@ int test_derivative_numerically(const bioslam::HingeJointConstraintNormErrEstAng
     bool testH2=gtsam::assert_equal(derivedH2,numericalH2,1e-6);
     bool testH3=gtsam::assert_equal(derivedH3,numericalH3,1e-6);
     bool testH4=gtsam::assert_equal(derivedH4,numericalH4,1e-6);
-    bool testH5=gtsam::assert_equal(derivedH5,numericalH5,1e-6);
+    bool testH5=gtsam::assert_equal(derivedH5,numericalH5,1e-5);
     if (!testH1){
         std::cerr<<"H1 did not check out numerically."<<std::endl<<"derivedH1="<<derivedH1<<std::endl<<"numericalH1"<<numericalH1<<std::endl;
         return 1;
@@ -239,7 +239,7 @@ int test_derivative_numerically(const bioslam::HingeJointConstraintNormErrEstAng
         std::cerr<<"H4 did not check out numerically."<<std::endl<<"derivedH4="<<derivedH4<<std::endl<<"numericalH4"<<numericalH4<<std::endl;
         return 1;
     }
-    if (!testH5){ // commented out for now because this doesn't pass. why? are there multiple equivalent unit3 bases?
+    if (!testH5){
         std::cerr<<"H5 did not check out numerically."<<std::endl<<"derivedH5="<<derivedH5<<std::endl<<"numericalH5"<<numericalH5<<std::endl;
         return 1;
     }

test/unit/testMagnetometerFactors.cpp

@@ -15,13 +15,15 @@
 #include "factors/MagPose3Factor.h"
 #include <gtsam/base/numericalDerivative.h>
 #include "testutils.h"
+#include "mathutils.h"
 
 std::vector<gtsam::Rot3> get_vector_orientation_from_Pose3_Values(gtsam::Values vals);
 std::vector<gtsam::Rot3> get_vector_orientation_from_Rot3_Values(gtsam::Values vals);
 std::vector<gtsam::Rot3> MagFactor1_only_estimation(gtsam::Point3 B_global, gtsam::SharedNoiseModel magNoiseModel, std::vector<gtsam::Point3> magMeas);
 std::vector<gtsam::Rot3> MagPose3Factor_only_estimation(gtsam::Point3 B_global, gtsam::SharedNoiseModel magNoiseModel, std::vector<gtsam::Point3> magMeas);
 uint random_factor_tests(uint nTests=1000);
 int test_derivative_numerically(const bioslam::MagPose3Factor& fac, const gtsam::Pose3 &x);
+void print_test_results(double err, uint iterations, const gtsam::Pose3& initPose, const gtsam::Pose3& finalPose, const gtsam::Vector3& measB, const gtsam::Vector3& bN);
 
 int main(){
     // ----------
@@ -102,6 +104,7 @@ uint random_factor_tests(uint nTests){
         vals.insert(poseKey,x);
         gtsam::Vector3 meas=testutils::randomVector3(); // random measurement
         gtsam::Vector3 bN=testutils::randomRot3()*meas; // random global mag
+        meas.normalize(); bN.normalize(); // normalize for numerical conditioning
         // construct MagPose3Factor
         bioslam::MagPose3Factor fac=bioslam::MagPose3Factor(poseKey,meas,bN.norm(),gtsam::Unit3(bN.normalized()),gtsam::Point3(0.0,0.0,0.0),noiseModel);
         test_derivative_numerically(fac,x);
@@ -110,23 +113,33 @@ uint random_factor_tests(uint nTests){
         graph.add(fac);
         gtsam::LevenbergMarquardtOptimizer optimizer(graph,vals);
         gtsam::Values estimate=optimizer.optimize();
-        // test what came out
+        // collect output
         gtsam::Pose3 estimatedPose=estimate.at<gtsam::Pose3>(poseKey);
         gtsam::Rot3 R_B_to_N=estimatedPose.rotation();
         gtsam::Vector3 measN=R_B_to_N*meas;
-        if(optimizer.error()>1.0e-10){
-            std::cerr<<"final converged error is "<<optimizer.error()<<", which is too high."<<std::endl;
-            throw std::runtime_error("test failed.");
-        }
-        if((measN-bN).norm()>1.0e-5){
-            std::cerr<<"meas[N]=["<<measN.transpose()<<"], global B=["<<bN.transpose()<<"] (diff = ["<<(measN-bN).transpose()<<"]"<<std::endl;
-            std::cerr<<"did not converge body frame measurement to global B"<<std::endl;
-            throw std::runtime_error("test failed.");
+        // test criteria: LM error < 1.0e-5 & norm diff b/w measN and bN small.
+        //    exception to test: if angle between measN and bN is ~180 deg, derivative becomes zero. ignore these cases.
+        double angBw=mathutils::unsignedAngle(measN,bN);
+        if(angBw<M_PI*.99){ // ignore the aforementioned rare case where bN and measN point in opposite directions
+            if(optimizer.error()>1.0e-5){
+                print_test_results(optimizer.error(),optimizer.iterations(),x,estimatedPose,meas,bN);
+                throw std::runtime_error("test failed: optimized error too high.");
+            }
+            if(angBw>1.0e-5){
+                print_test_results(optimizer.error(),optimizer.iterations(),x,estimatedPose,meas,bN);
+                throw std::runtime_error("test failed: angle between measN and global mag def too large.");
+            }
         }
     }
     return 0;
 }
 
+void print_test_results(double err, uint iterations, const gtsam::Pose3& initPose, const gtsam::Pose3& finalPose, const gtsam::Vector3& measB, const gtsam::Vector3& bN){
+    gtsam::Vector3 measN=finalPose.rotation()*measB;
+    std::cout<<"optimizer: converged error "<<err<<" in "<<iterations<<" iterations"<<std::endl;
+    std::cout<<"meas[N]=["<<measN.transpose()<<"], global B=["<<bN.transpose()<<"] (diff = ["<<(measN-bN).transpose()<<"], norm="<<(measN-bN).norm()<<", angle b/w="<<mathutils::unsignedAngle(measN,bN)<<")"<<std::endl;
+}
+
 int test_derivative_numerically(const bioslam::MagPose3Factor& fac, const gtsam::Pose3 &x){
     // unit test the Jacobian against GTSAM's numerical derivative
     // () get derived error and jacobians
@@ -209,4 +222,4 @@ std::vector<gtsam::Rot3> get_vector_orientation_from_Pose3_Values(gtsam::Values
         i++;
     }
     return vector_Rot3;
-}
+}

test/unit/testPose3CompassPrior.cpp

@@ -66,15 +66,23 @@ int random_factor_tests(uint nTests){
     // (note: when sigma=1, then optimizer error = 0.5*(factor error^2)
     for(uint i=0;i<nTests;i++) {
         gtsam::Pose3 x=testutils::randomPose3();
-        double prior=testutils::dRand(-M_PI,M_PI);
+        double prior=testutils::dRand(-M_PI*0.75,M_PI*0.75);
+        gtsam::Vector3 refVecLocal=gtsam::Vector3(1.0,0.0,0.0);
+        gtsam::Vector3 refVecNav=gtsam::Vector3(0.0,0.0,1.0);
+        gtsam::Vector3 upVecNav=gtsam::Vector3(0.0,0.0,1.0);
         if (!myVals.exists(xKey)) {
             myVals.insert(xKey, x);
         } else {
             myVals.update(xKey, x);
         }
-        bioslam::Pose3CompassPrior testFac=bioslam::Pose3CompassPrior(xKey,gtsam::Vector3(0.0,0.0,1.0),gtsam::Vector3(1.0,0.0,0.0),gtsam::Vector3(0.0,0.0,1.0),prior,myNoiseModel);
+        bioslam::Pose3CompassPrior testFac=bioslam::Pose3CompassPrior(xKey,refVecLocal,refVecNav,upVecNav,prior,myNoiseModel);
         // test derivative numerically
-        test_derivative_numerically(testFac, x);
+        if (!bioslam::Pose3CompassPrior::isCompassVertical(x.rotation(),refVecLocal,upVecNav)){
+            // only run test if this random system isn't at the gimbal lock condition
+            test_derivative_numerically(testFac, x);
+        }else{
+            std::cout<<"randomly generated compass system was at gimbal lock condition; skipping numerical testing of jacobian."<<std::endl;
+        }
     }
     return 0;
 }
@@ -90,14 +98,13 @@ int test_derivative_numerically(const bioslam::Pose3CompassPrior& fac, const gts
     //    templates are: <output type (typically gtsam::Vector), then the input argument types in order)
     gtsam::Matrix numericalH1=gtsam::numericalDerivative11<gtsam::Vector,gtsam::Pose3>(
             boost::function<gtsam::Vector(const gtsam::Pose3&)>
-                    (boost::bind(&bioslam::Pose3CompassPrior::evaluateError,fac,_1,boost::none)),x,1e-5);
+                    (boost::bind(&bioslam::Pose3CompassPrior::evaluateError,fac,_1,boost::none)),x,1e-8);
 
     // now test using gtsam::assert_equal()
     bool testH1=gtsam::assert_equal(derivedH1,numericalH1,5.0e-3);
     if (!testH1){
         std::cerr<<"H1 did not check out numerically."<<std::endl<<"derivedH1="<<derivedH1<<std::endl<<"numericalH1"<<numericalH1<<std::endl;
         throw std::runtime_error("Jacobian did not check out numerically");
-        return 1;
     }
     return 0;
 }