diff --git a/.gitignore b/.gitignore
index 70c546a4..35c8c65f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -29,3 +29,5 @@
 
 % Ignore python bytecode
 *.pyc
+
+*compile_commands.json
diff --git a/Dockerfile_18_04 b/Dockerfile_18_04
deleted file mode 100644
index 98a93ed1..00000000
--- a/Dockerfile_18_04
+++ /dev/null
@@ -1,128 +0,0 @@
-# Use an official Python runtime as a parent image
-FROM ubuntu:18.04
-
-MAINTAINER Antoni Rosinol "arosinol@mit.edu"
-
-# To avoid tzdata asking for geographic location...
-ENV DEBIAN_FRONTEND=noninteractive
-
-# Set the working directory to /root
-ENV DIRPATH /root/
-WORKDIR $DIRPATH
-
-#Install build dependencies
-RUN apt-get update && apt-get install -y --no-install-recommends apt-utils
-RUN apt-get update && apt-get install -y git cmake build-essential pkg-config
-
-# Install xvfb to provide a display to container for GUI realted testing.
-RUN apt-get update && apt-get install -y xvfb
-
-# Install OpenCV for Ubuntu 18.04
-RUN apt-get update && apt-get install -y \
-      unzip \
-      libjpeg-dev \
-      libpng-dev \
-      libtiff-dev \
-      libvtk6-dev \
-      libgtk-3-dev \
-      libatlas-base-dev \
-      gfortran
-
-RUN git clone https://github.com/opencv/opencv.git
-RUN cd opencv && \
-      git checkout tags/3.3.1 && \
-      mkdir build
-
-RUN git clone https://github.com/opencv/opencv_contrib.git
-RUN cd opencv_contrib && \
-      git checkout tags/3.3.1
-
-RUN cd opencv/build && \
-      cmake -DCMAKE_BUILD_TYPE=Release \
-      -DCMAKE_INSTALL_PREFIX=/usr/local \
-      -D BUILD_opencv_python=OFF \
-      -D BUILD_opencv_python2=OFF \
-      -D BUILD_opencv_python3=OFF \
-      -DOPENCV_EXTRA_MODULES_PATH=$DIRPATH/opencv_contrib/modules .. && \
-      make -j$(nproc) install
-
-# Install GTSAM
-RUN apt-get update && apt-get install -y libboost-all-dev libtbb-dev
-ADD https://api.github.com/repos/borglab/gtsam/git/refs/heads/develop version.json
-RUN git clone https://github.com/borglab/gtsam.git
-RUN cd gtsam && \
-    git fetch && \
-    git checkout e5866799dff48239573cdd84964180867e50edd2 && \
-    mkdir build && \
-    cd build && \
-    cmake -DCMAKE_INSTALL_PREFIX=/usr/local -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_UNSTABLE=ON -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_TANGENT_PREINTEGRATION=OFF .. && \
-    make -j$(nproc) install
-
-# Install Open_GV
-RUN git clone https://github.com/laurentkneip/opengv
-RUN cd opengv && \
-      mkdir build
-RUN cd opengv/build && \
-      cmake -DCMAKE_BUILD_TYPE=Release \
-      -DCMAKE_INSTALL_PREFIX=/usr/local \
-      -DEIGEN_INCLUDE_DIRS=$DIRPATH/gtsam/gtsam/3rdparty/Eigen \
-      -DEIGEN_INCLUDE_DIR=$DIRPATH/gtsam/gtsam/3rdparty/Eigen .. && \
-      make -j$(nproc) install
-
-# Install DBoW2
-RUN git clone https://github.com/dorian3d/DBoW2.git
-RUN cd DBoW2 && \
-      mkdir build && \
-      cd build && \
-      cmake .. && \
-      make -j$(nproc) install
-
-# Install RobustPGO
-ADD https://api.github.com/repos/MIT-SPARK/Kimera-RPGO/git/refs/heads/master version.json
-RUN git clone https://github.com/MIT-SPARK/Kimera-RPGO.git
-RUN cd Kimera-RPGO && \
-      mkdir build && \
-      cd build && \
-      cmake .. && \
-      make -j$(nproc)
-
-RUN apt-get update && \
-    apt-get install software-properties-common -y
-# Get python3
-RUN apt-get update && \
-    add-apt-repository ppa:deadsnakes/ppa
-RUN apt-get update && \
-    apt-get install -y python3.6 python3.6-dev python-pip python3-pip python-tk python3-tk
-# Upgrade pip to fix dependency issue with Pillow
-RUN python3.6 $(which pip3) install --upgrade pip
-RUN python3.6 $(which pip3) install PyQt5==5.14
-
-# Install evo-1 for evaluation
-# Hack to avoid Docker's cache when evo-1 master branch is updated.
-ADD https://api.github.com/repos/MIT-SPARK/evo-1/git/refs/heads/master version.json
-RUN git clone https://github.com/MIT-SPARK/evo-1.git
-RUN cd evo-1 && python3.6 $(which pip3) install .
-
-# Install Kimera VIO Evaluation
-RUN python3.6 $(which pip3) install ipython prompt_toolkit
-# Hack to avoid Docker's cache when Kimera VIO Evaluation jenkins/base branch is updated.
-ADD https://api.github.com/repos/MIT-SPARK/Kimera-VIO-Evaluation/git/refs/heads/jenkins/base version.json
-RUN git clone https://github.com/MIT-SPARK/Kimera-VIO-Evaluation.git
-# We use `pip3 install -e .` so that Jinja2 has access to the webiste template...
-RUN cd Kimera-VIO-Evaluation && git fetch && git checkout jenkins/base && python3.6 $(which pip3) install -e .
-
-# Install glog, gflags
-RUN apt-get update && apt-get install -y libgflags2.2 libgflags-dev libgoogle-glog0v5 libgoogle-glog-dev
-
-# Install Pangolin
-RUN apt-get update && apt-get install -y libgl1-mesa-dev libglew-dev
-RUN git clone --depth 1 --branch v0.6 https://github.com/stevenlovegrove/Pangolin.git
-RUN cd Pangolin && \
-      mkdir build && \
-      cd build && \
-      cmake .. && \
-      make -j$(nproc)
-
-# Clean
-RUN apt-get clean && \
-      rm -rf /var/lib/apt/lists/*
diff --git a/Dockerfile_20_04 b/Dockerfile_20_04
index da91d265..b5175027 100644
--- a/Dockerfile_20_04
+++ b/Dockerfile_20_04
@@ -1,134 +1,84 @@
-# Use an official Python runtime as a parent image
 FROM ubuntu:20.04
-
 LABEL maintainer="mabate@mit.edu"
 
 # To avoid tzdata asking for geographic location...
 ENV DEBIAN_FRONTEND=noninteractive
 
-# Set the working directory to /root
 ENV DIRPATH /root/
 WORKDIR $DIRPATH
 
-#Install build dependencies
 RUN apt-get update && apt-get install -y --no-install-recommends apt-utils
-RUN apt-get update && apt-get install -y git cmake build-essential pkg-config
-
-# Install xvfb to provide a display to container for GUI realted testing.
-RUN apt-get update && apt-get install -y xvfb
-
-# Install OpenCV for Ubuntu 20.04
-RUN apt-get update && apt-get install -y \
-      unzip \
-      libjpeg-dev \
-      libpng-dev \
-      libtiff-dev \
-      libvtk7-dev \
-      libgtk-3-dev \
+RUN apt-get update && \
+    apt-get install -y \
+      build-essential \
+      cmake \
+      gfortran \
+      git \
       libatlas-base-dev \
-      gfortran
-
-RUN git clone https://github.com/opencv/opencv.git
-RUN cd opencv && \
-      git checkout tags/3.4.2 && \
-      mkdir build
-
-RUN git clone https://github.com/opencv/opencv_contrib.git
-RUN cd opencv_contrib && \
-      git checkout tags/3.4.2
-
-RUN cd opencv/build && \
-      cmake -DCMAKE_BUILD_TYPE=Release \
-      -DCMAKE_INSTALL_PREFIX=/usr/local \
-      -D BUILD_opencv_python=OFF \
-      -D BUILD_opencv_python2=OFF \
-      -D BUILD_opencv_python3=OFF \
-      -DOPENCV_EXTRA_MODULES_PATH=$DIRPATH/opencv_contrib/modules .. && \
-      make -j$(nproc) install
+      libboost-all-dev \
+      libeigen3-dev \
+      libgflags-dev \
+      libgoogle-glog-dev \
+      libopencv-dev \
+      libopencv-contrib-dev \
+      libtbb-dev \
+      pkg-config \
+      xvfb \
+      python3 \
+      python3-dev \
+      python3-pip \
+      python3-tk && \
+    apt-get clean && \
+    rm -rf /var/lib/apt/lists/*
 
-# Install GTSAM
-RUN apt-get update && apt-get install -y libboost-all-dev libtbb-dev
 ADD https://api.github.com/repos/borglab/gtsam/git/refs/heads/develop version.json
-RUN git clone https://github.com/borglab/gtsam.git
-RUN cd gtsam && \
-    git fetch && \
-    git checkout e5866799dff48239573cdd84964180867e50edd2 && \
+RUN git clone https://github.com/borglab/gtsam.git && \
+    cd gtsam && \
+    git checkout 4.2 && \
     mkdir build && \
     cd build && \
-    cmake -DCMAKE_INSTALL_PREFIX=/usr/local -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_UNSTABLE=ON -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_TANGENT_PREINTEGRATION=OFF .. && \
+    cmake -DCMAKE_INSTALL_PREFIX=/usr/local \
+          -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
+          -DGTSAM_BUILD_TESTS=OFF \
+          -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \
+          -DCMAKE_BUILD_TYPE=Release \
+          -DGTSAM_BUILD_UNSTABLE=ON \
+          -DGTSAM_POSE3_EXPMAP=ON \
+          -DGTSAM_ROT3_EXPMAP=ON \
+          -DGTSAM_TANGENT_PREINTEGRATION=OFF \
+          -DGTSAM_USE_SYSTEM_EIGEN=ON \
+          .. && \
     make -j$(nproc) install
 
 # Install Open_GV
-RUN git clone https://github.com/marcusabate/opengv
-RUN cd opengv && git apply march_native_disable.patch && \
-      mkdir build
-RUN cd opengv/build && \
-      cmake -DCMAKE_BUILD_TYPE=Release \
-      -DCMAKE_INSTALL_PREFIX=/usr/local \
-      -DEIGEN_INCLUDE_DIRS=$DIRPATH/gtsam/gtsam/3rdparty/Eigen \
-      -DEIGEN_INCLUDE_DIR=$DIRPATH/gtsam/gtsam/3rdparty/Eigen .. && \
-      make -j$(nproc) install
+RUN git clone https://github.com/marcusabate/opengv && \
+    cd opengv && \
+    git apply march_native_disable.patch && \
+    mkdir build && \
+    cd build && \
+    cmake -DCMAKE_BUILD_TYPE=Release \
+          -DCMAKE_INSTALL_PREFIX=/usr/local \
+          .. && \
+    make -j$(nproc) install
 
 # Install DBoW2
 RUN git clone https://github.com/dorian3d/DBoW2.git
 RUN cd DBoW2 && \
-      mkdir build && \
-      cd build && \
-      cmake .. && \
-      make -j$(nproc) install
+    mkdir build && \
+    cd build && \
+    cmake .. && \
+    make -j$(nproc) install
 
 # Install RobustPGO
 ADD https://api.github.com/repos/MIT-SPARK/Kimera-RPGO/git/refs/heads/master version.json
 RUN git clone https://github.com/MIT-SPARK/Kimera-RPGO.git
 RUN cd Kimera-RPGO && \
-      mkdir build && \
-      cd build && \
-      cmake .. && \
-      make -j$(nproc)
-
-RUN apt-get update && \
-      apt-get install -y software-properties-common
-
-# Get python3
-RUN apt-get update && \
-      apt-get install -y python3.8 python3.8-dev python3-pip python3-tk
-RUN apt-get update && \
-      apt-get install -y libgl1-mesa-glx
-RUN python3 $(which pip3) install --upgrade pip
-RUN python3 $(which pip3) install PyQt5
-
-# Install evo-1 for evaluation
-# Hack to avoid Docker's cache when evo-1 fix/python3 branch is updated.
-RUN python3 $(which pip3) install open3d plotly --upgrade
-RUN python3 $(which pip3) install setuptools==58.3.0
-ADD https://api.github.com/repos/MIT-SPARK/evo-1/git/refs/heads/fix/python3 version.json
-RUN git clone https://github.com/MIT-SPARK/evo-1.git
-RUN cd evo-1 && git checkout fix/python3 && python3.8 $(which pip3) install .
-
-# Install Kimera VIO Evaluation
-RUN python3 $(which pip3) install ipython prompt_toolkit
-# Hack to avoid Docker's cache when Kimera VIO Evaluation fix/python3 branch is updated.
-ADD https://api.github.com/repos/marcusabate/Kimera-VIO-Evaluation/git/refs/heads/fix/jenkins_tests/python3_lite version.json
-RUN git clone https://github.com/marcusabate/Kimera-VIO-Evaluation.git
-# We do the following because of a 20.04 issue with .egg-link file being in site-packages instead of dist-packages
-RUN rm -rf /usr/lib/python3.8/site-packages/ && \
-      ln -s /usr/local/lib/python3.8/dist-packages /usr/lib/python3.8/site-packages
-# We use `pip3 install -e .` so that Jinja2 has access to the webiste template...
-RUN cd Kimera-VIO-Evaluation && git fetch && git checkout fix/jenkins_tests/python3_lite && python3 $(which pip3) install -e .
-
-# Install glog, gflags
-RUN apt-get update && apt-get install -y libgflags2.2 libgflags-dev libgoogle-glog0v5 libgoogle-glog-dev
-
-# Install Pangolin
-RUN apt-get update && apt-get install -y libgl1-mesa-dev libglew-dev
-RUN git clone --depth 1 --branch v0.6 https://github.com/stevenlovegrove/Pangolin.git
-RUN cd Pangolin && \
-      mkdir build && \
-      cd build && \
-      cmake .. && \
-      make -j$(nproc)
-
-# Clean
-RUN apt-get clean && \
-      rm -rf /var/lib/apt/lists/*
+    mkdir build && \
+    cd build && \
+    cmake .. && \
+    make -j$(nproc) install
 
+# Install Kimera-VIO-Evaluation
+RUN python3 -m pip install --upgrade pip
+ADD https://api.github.com/repos/marcusabate/Kimera-VIO-Evaluation/git/refs/heads/fix/python3 version.json
+RUN python3 -m pip install git+https://github.com/MIT-SPARK/Kimera-VIO-Evaluation.git@fix/python3
diff --git a/Jenkinsfile b/Jenkinsfile
index 186e0ab0..eed0f833 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -21,12 +21,12 @@ pipeline {
   stages {
     stage('Build and Test on Ubuntu') {
       parallel {
-        stage('Ubuntu 18.04') {
+        stage('Ubuntu 20.04') {
           agent {
               dockerfile {
-                filename 'Dockerfile_18_04'
-                  args '-e WORKSPACE=$WORKSPACE'
-              }
+                filename 'Dockerfile_20_04'
+                args '-e WORKSPACE=$WORKSPACE'
+            }
           }
           environment {
             evaluator="/root/Kimera-VIO-Evaluation"
@@ -34,34 +34,32 @@ pipeline {
           stages {
             stage('Build Release') {
               steps {
-               cmakeBuild buildDir: 'build', buildType: 'Release', cleanBuild: false,
-                          cmakeArgs: '-DEIGEN3_INCLUDE_DIR="/usr/local/include/gtsam/3rdparty/Eigen"\
-                            -DCMAKE_CXX_FLAGS="\
-                            -Wno-comment \
-                            -Wno-maybe-uninitialized \
-                            -Wno-parentheses \
-                            -Wno-pragma-once-outside-header \
-                            -Wno-reorder \
-                            -Wno-return-type \
-                            -Wno-sign-compare \
-                            -Wno-unused-but-set-variable \
-                            -Wno-unused-function \
-                            -Wno-unused-parameter \
-                            -Wno-unused-value \
-                            -Wno-unused-variable"',
-                          generator: 'Unix Makefiles', installation: 'InSearchPath',
-                          sourceDir: '.', steps: [[args: '-j 4']]
+                slackSend color: 'good',
+                          message: "Started Build <${env.BUILD_URL}|#${env.BUILD_NUMBER}> - Branch <${env.GIT_URL}|${env.GIT_BRANCH}>."
+                cmakeBuild buildDir: 'build', buildType: 'Release', cleanBuild: false,
+                           cmakeArgs: '-DEIGEN3_INCLUDE_DIR="/usr/local/include/gtsam/3rdparty/Eigen"\
+                             -DCMAKE_CXX_FLAGS="\
+                             -Wno-comment \
+                             -Wno-maybe-uninitialized \
+                             -Wno-parentheses \
+                             -Wno-pragma-once-outside-header \
+                             -Wno-reorder \
+                             -Wno-return-type \
+                             -Wno-sign-compare \
+                             -Wno-unused-but-set-variable \
+                             -Wno-unused-function \
+                             -Wno-unused-parameter \
+                             -Wno-unused-value \
+                             -Wno-unused-variable"',
+                           generator: 'Unix Makefiles', installation: 'InSearchPath',
+                           sourceDir: '.', steps: [[args: '-j 4']]
               }
             }
             stage('Test') {
               steps {
-                catchError(buildResult: 'SUCCESS', stageResult: 'FAILURE') {
-                  wrap([$class: 'Xvfb']) {
-                    sh 'cd build && ./testKimeraVIO --gtest_output="xml:testresults.xml" --minloglevel=0 -v=1'
-
-                    // Process the CTest xml output
-                    junit 'build/testresults.xml'
-                  }
+                wrap([$class: 'Xvfb']) {
+                  sh 'cd build && ./testKimeraVIO --gtest_output="xml:testresults.xml" --minloglevel=0 -v=1'
+                  junit 'build/testresults.xml'
                 }
               }
             }
@@ -75,12 +73,12 @@ pipeline {
                   sh 'evo_config set plot_split false'
 
                   // 2. Run evaluation
-                  sh 'python3.6 $evaluator/evaluation/main_evaluation.py -r -a -v \
+                  sh 'python3 $evaluator/evaluation/main_evaluation.py -r -a -v \
                     --save_plots --save_boxplots --save_results --write_website \
                     $evaluator/experiments/jenkins_euroc.yaml'
 
                   // 3. Compile summary results.
-                  sh 'python3.6 $evaluator/evaluation/tools/performance_summary.py \
+                  sh 'python3 $evaluator/evaluation/tools/performance_summary.py \
                     $WORKSPACE/website/data/V1_01_easy/Euroc/results_vio.yaml \
                     $WORKSPACE/website/data/V1_01_easy/Euroc/vio_performance.csv'
                 }
@@ -131,52 +129,6 @@ pipeline {
             }
           }
         }
-        stage('Ubuntu 20.04') {
-          agent {
-              dockerfile {
-                filename 'Dockerfile_20_04'
-                  args '-e WORKSPACE=$WORKSPACE'
-            }
-          }
-          environment {
-            evaluator="/root/Kimera-VIO-Evaluation"
-          }
-          stages {
-            stage('Build Release') {
-              steps {
-                slackSend color: 'good',
-                          message: "Started Build <${env.BUILD_URL}|#${env.BUILD_NUMBER}> - Branch <${env.GIT_URL}|${env.GIT_BRANCH}>."
-               cmakeBuild buildDir: 'build', buildType: 'Release', cleanBuild: false,
-                          cmakeArgs: '-DEIGEN3_INCLUDE_DIR="/usr/local/include/gtsam/3rdparty/Eigen"\
-                            -DCMAKE_CXX_FLAGS="\
-                            -Wno-comment \
-                            -Wno-maybe-uninitialized \
-                            -Wno-parentheses \
-                            -Wno-pragma-once-outside-header \
-                            -Wno-reorder \
-                            -Wno-return-type \
-                            -Wno-sign-compare \
-                            -Wno-unused-but-set-variable \
-                            -Wno-unused-function \
-                            -Wno-unused-parameter \
-                            -Wno-unused-value \
-                            -Wno-unused-variable"',
-                          generator: 'Unix Makefiles', installation: 'InSearchPath',
-                          sourceDir: '.', steps: [[args: '-j 4']]
-              }
-            }
-            stage('Test') {
-              steps {
-                wrap([$class: 'Xvfb']) {
-                  sh 'cd build && ./testKimeraVIO --gtest_output="xml:testresults.xml" --minloglevel=0 -v=1'
-
-                  // Process the CTest xml output
-                  junit 'build/testresults.xml'
-                }
-              }
-            }
-          }
-        }
       }
     }
   }
@@ -184,8 +136,6 @@ pipeline {
     always {
       node(null) {
         echo 'Jenkins Finished'
-
-
         // Clear the source and build dirs before the next run
         // deleteDir()
       }