diff --git a/CMakeLists.txt b/CMakeLists.txt
index 08a8af468e..d8db0877b6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,7 +28,7 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
-find_package(OpenCV 3.0 QUIET)
+find_package(OpenCV 4 QUIET)
 if(NOT OpenCV_FOUND)
    find_package(OpenCV 2.4.3 QUIET)
    if(NOT OpenCV_FOUND)
diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc
index 4bcb90f170..00175ff39b 100644
--- a/Examples/Monocular/mono_euroc.cc
+++ b/Examples/Monocular/mono_euroc.cc
@@ -23,8 +23,10 @@
 #include<algorithm>
 #include<fstream>
 #include<chrono>
+#include<unistd.h>
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgcodecs/legacy/constants_c.h>
 
 #include<System.h>
 
diff --git a/Examples/Monocular/mono_kitti.cc b/Examples/Monocular/mono_kitti.cc
index f2f7b3ebfd..c4d9fbd9fc 100644
--- a/Examples/Monocular/mono_kitti.cc
+++ b/Examples/Monocular/mono_kitti.cc
@@ -24,8 +24,10 @@
 #include<fstream>
 #include<chrono>
 #include<iomanip>
+#include<unistd.h>
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgcodecs/legacy/constants_c.h>
 
 #include"System.h"
 
diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc
index 09a2afc4b7..5c6e68c811 100644
--- a/Examples/Monocular/mono_tum.cc
+++ b/Examples/Monocular/mono_tum.cc
@@ -23,8 +23,10 @@
 #include<algorithm>
 #include<fstream>
 #include<chrono>
+#include<unistd.h>
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgcodecs/legacy/constants_c.h>
 
 #include<System.h>
 
diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc
index 001199d1aa..53d41d202a 100644
--- a/Examples/RGB-D/rgbd_tum.cc
+++ b/Examples/RGB-D/rgbd_tum.cc
@@ -23,8 +23,10 @@
 #include<algorithm>
 #include<fstream>
 #include<chrono>
+#include <unistd.h>
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgcodecs/legacy/constants_c.h>
 
 #include<System.h>
 
diff --git a/Examples/ROS/ORB_SLAM2/CMakeLists.txt b/Examples/ROS/ORB_SLAM2/CMakeLists.txt
index 7cbf71c199..d25c873cdd 100644
--- a/Examples/ROS/ORB_SLAM2/CMakeLists.txt
+++ b/Examples/ROS/ORB_SLAM2/CMakeLists.txt
@@ -30,7 +30,7 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
 
-find_package(OpenCV 3.0 QUIET)
+find_package(OpenCV 4 QUIET)
 if(NOT OpenCV_FOUND)
    find_package(OpenCV 2.4.3 QUIET)
    if(NOT OpenCV_FOUND)
diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc
index 6bc09c50be..b6149f6c67 100644
--- a/Examples/Stereo/stereo_euroc.cc
+++ b/Examples/Stereo/stereo_euroc.cc
@@ -24,8 +24,10 @@
 #include<fstream>
 #include<iomanip>
 #include<chrono>
+#include<unistd.h>
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgcodecs/legacy/constants_c.h>
 
 #include<System.h>
 
diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc
index cb8bc4096e..5fd55a3e35 100644
--- a/Examples/Stereo/stereo_kitti.cc
+++ b/Examples/Stereo/stereo_kitti.cc
@@ -24,8 +24,10 @@
 #include<fstream>
 #include<iomanip>
 #include<chrono>
+#include<unistd.h>
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgcodecs/legacy/constants_c.h>
 
 #include<System.h>
 
diff --git a/Thirdparty/DBoW2/CMakeLists.txt b/Thirdparty/DBoW2/CMakeLists.txt
index 0eb512600a..8b1061f1f4 100644
--- a/Thirdparty/DBoW2/CMakeLists.txt
+++ b/Thirdparty/DBoW2/CMakeLists.txt
@@ -24,7 +24,7 @@ set(SRCS_DUTILS
   DUtils/Random.cpp
   DUtils/Timestamp.cpp)
 
-find_package(OpenCV 3.0 QUIET)
+find_package(OpenCV 4 QUIET)
 if(NOT OpenCV_FOUND)
    find_package(OpenCV 2.4.3 QUIET)
    if(NOT OpenCV_FOUND)
diff --git a/gen_imp.sh b/gen_imp.sh
new file mode 100644
index 0000000000..bd29f87311
--- /dev/null
+++ b/gen_imp.sh
@@ -0,0 +1,16 @@
+dir="`pwd`"
+
+echo '['
+
+pushd /usr/include/eigen3/Eigen &> /dev/null
+for i in *; do echo "  { include: ['@<Eigen/src/$i/.*>', 'private', '<Eigen/$i>', 'public'] },"; done | sed s/\'/\"/g
+popd &>/dev/null
+
+echo
+
+pushd /usr/include/opencv4/opencv2 &>/dev/null
+for i in *.hpp; do echo "  { include: ['@<opencv2/${i%.hpp}/.*.hpp>', 'private', '<opencv2/$i>', 'public'] },"; done | sed s/\'/\"/g
+popd &> /dev/null
+
+echo '  { include: ["<this is a dummy that does not exist>", "private", "<blah>", "public"] }'
+echo ']'
diff --git a/include/Frame.h b/include/Frame.h
index a6a8032f57..176a7f5de9 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -23,6 +23,10 @@
 
 #include<vector>
 
+#include <opencv2/opencv.hpp>
+#include <opencv2/core.hpp>
+#include <cstddef>
+
 #include "MapPoint.h"
 #include "Thirdparty/DBoW2/DBoW2/BowVector.h"
 #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
@@ -30,7 +34,7 @@
 #include "KeyFrame.h"
 #include "ORBextractor.h"
 
-#include <opencv2/opencv.hpp>
+namespace ORB_SLAM2 { class ORBextractor; }
 
 namespace ORB_SLAM2
 {
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index 67f4348273..71e3a5db5c 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -21,6 +21,13 @@
 #ifndef KEYFRAME_H
 #define KEYFRAME_H
 
+#include <stddef.h>
+#include <opencv2/core.hpp>
+#include <mutex>
+#include <map>
+#include <set>
+#include <vector>
+
 #include "MapPoint.h"
 #include "Thirdparty/DBoW2/DBoW2/BowVector.h"
 #include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
@@ -29,8 +36,6 @@
 #include "Frame.h"
 #include "KeyFrameDatabase.h"
 
-#include <mutex>
-
 
 namespace ORB_SLAM2
 {
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
index 1361a5757a..5cf34dccd6 100644
--- a/include/LocalMapping.h
+++ b/include/LocalMapping.h
@@ -21,14 +21,16 @@
 #ifndef LOCALMAPPING_H
 #define LOCALMAPPING_H
 
+#include <opencv2/core.hpp>
+#include <mutex>
+#include <list>
+
 #include "KeyFrame.h"
 #include "Map.h"
 #include "LoopClosing.h"
 #include "Tracking.h"
 #include "KeyFrameDatabase.h"
 
-#include <mutex>
-
 
 namespace ORB_SLAM2
 {
@@ -36,6 +38,8 @@ namespace ORB_SLAM2
 class Tracking;
 class LoopClosing;
 class Map;
+class KeyFrame;
+class MapPoint;
 
 class LocalMapping
 {
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
index 7eb0416b15..abcc167c48 100644
--- a/include/LoopClosing.h
+++ b/include/LoopClosing.h
@@ -21,17 +21,25 @@
 #ifndef LOOPCLOSING_H
 #define LOOPCLOSING_H
 
+#include <opencv2/core.hpp>
+#include <thread>
+#include <mutex>
+#include <Eigen/Core>
+#include <functional>
+#include <list>
+#include <map>
+#include <set>
+#include <utility>
+#include <vector>
+
 #include "KeyFrame.h"
 #include "LocalMapping.h"
 #include "Map.h"
 #include "ORBVocabulary.h"
 #include "Tracking.h"
-
 #include "KeyFrameDatabase.h"
-
-#include <thread>
-#include <mutex>
 #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+#include "Thirdparty/g2o/g2o/types/sim3.h"
 
 namespace ORB_SLAM2
 {
@@ -39,6 +47,9 @@ namespace ORB_SLAM2
 class Tracking;
 class LocalMapping;
 class KeyFrameDatabase;
+class KeyFrame;
+class Map;
+class MapPoint;
 
 
 class LoopClosing
@@ -47,7 +58,7 @@ class LoopClosing
 
     typedef pair<set<KeyFrame*>,int> ConsistentGroup;    
     typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
-        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
+        Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;
 
 public:
 
diff --git a/include/Map.h b/include/Map.h
index a75339feea..53f2c8ca75 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -21,11 +21,12 @@
 #ifndef MAP_H
 #define MAP_H
 
-#include "MapPoint.h"
-#include "KeyFrame.h"
 #include <set>
-
 #include <mutex>
+#include <vector>
+
+#include "MapPoint.h"
+#include "KeyFrame.h"
 
 
 
diff --git a/include/ORBextractor.h b/include/ORBextractor.h
index 66e8e7a547..b4e244d9d4 100644
--- a/include/ORBextractor.h
+++ b/include/ORBextractor.h
@@ -23,7 +23,7 @@
 
 #include <vector>
 #include <list>
-#include <opencv/cv.h>
+#include <opencv2/core.hpp>
 
 
 namespace ORB_SLAM2
diff --git a/include/Optimizer.h b/include/Optimizer.h
index 2c4378ed3b..0aca5822fb 100644
--- a/include/Optimizer.h
+++ b/include/Optimizer.h
@@ -21,18 +21,28 @@
 #ifndef OPTIMIZER_H
 #define OPTIMIZER_H
 
+#include <stddef.h>
+#include <map>
+#include <set>
+#include <vector>
+
 #include "Map.h"
 #include "MapPoint.h"
 #include "KeyFrame.h"
 #include "LoopClosing.h"
 #include "Frame.h"
-
 #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
 
+namespace g2o { struct Sim3; }
+
 namespace ORB_SLAM2
 {
 
 class LoopClosing;
+class Frame;
+class KeyFrame;
+class Map;
+class MapPoint;
 
 class Optimizer
 {
diff --git a/include/PnPsolver.h b/include/PnPsolver.h
index f92544fc8f..5558dcbd41 100644
--- a/include/PnPsolver.h
+++ b/include/PnPsolver.h
@@ -52,11 +52,18 @@
 #define PNPSOLVER_H
 
 #include <opencv2/core/core.hpp>
+#include <opencv2/core/core_c.h>
+#include <stddef.h>
+#include <opencv2/core.hpp>
+#include <vector>
+
 #include "MapPoint.h"
 #include "Frame.h"
 
 namespace ORB_SLAM2
 {
+class Frame;
+class MapPoint;
 
 class PnPsolver {
  public:
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index 9af66cb26f..35bb0882b4 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -23,6 +23,8 @@
 #define SIM3SOLVER_H
 
 #include <opencv2/opencv.hpp>
+#include <stddef.h>
+#include <opencv2/core.hpp>
 #include <vector>
 
 #include "KeyFrame.h"
@@ -31,6 +33,8 @@
 
 namespace ORB_SLAM2
 {
+class KeyFrame;
+class MapPoint;
 
 class Sim3Solver
 {
diff --git a/include/Tracking.h b/include/Tracking.h
index 5aaa93ef26..ecfea4075b 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -23,6 +23,7 @@
 #define TRACKING_H
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgproc/imgproc_c.h>
 #include<opencv2/features2d/features2d.hpp>
 
 #include"Viewer.h"
@@ -105,10 +106,14 @@ class Tracking
 
     // Lists used to recover the full camera trajectory at the end of the execution.
     // Basically we store the reference keyframe for each frame and its relative transformation
-    list<cv::Mat> mlRelativeFramePoses;
-    list<KeyFrame*> mlpReferences;
-    list<double> mlFrameTimes;
-    list<bool> mlbLost;
+    struct TrackedFrame
+    {
+        cv::Mat relative_frame_pose;
+        KeyFrame* reference_keyframe;
+        double time;
+        bool lost;
+    };
+    list<TrackedFrame> tracked_frames;
 
     // True if local mapping is deactivated and we are performing only localization
     bool mbOnlyTracking;
diff --git a/include/Viewer.h b/include/Viewer.h
index 251e223c54..c842ed4965 100644
--- a/include/Viewer.h
+++ b/include/Viewer.h
@@ -22,13 +22,14 @@
 #ifndef VIEWER_H
 #define VIEWER_H
 
+#include <mutex>
+#include <string>
+
 #include "FrameDrawer.h"
 #include "MapDrawer.h"
 #include "Tracking.h"
 #include "System.h"
 
-#include <mutex>
-
 namespace ORB_SLAM2
 {
 
diff --git a/iwyu.imp b/iwyu.imp
new file mode 100644
index 0000000000..7d8634b552
--- /dev/null
+++ b/iwyu.imp
@@ -0,0 +1,82 @@
+[
+{ include: ["@<Eigen/src/Cholesky/.*>", "private", "<Eigen/Cholesky>", "public"] },
+{ include: ["@<Eigen/src/CholmodSupport/.*>", "private", "<Eigen/CholmodSupport>", "public"] },
+{ include: ["@<Eigen/src/Core/.*>", "private", "<Eigen/Core>", "public"] },
+{ include: ["@<Eigen/src/Dense/.*>", "private", "<Eigen/Dense>", "public"] },
+{ include: ["@<Eigen/src/Eigen/.*>", "private", "<Eigen/Eigen>", "public"] },
+{ include: ["@<Eigen/src/Eigenvalues/.*>", "private", "<Eigen/Eigenvalues>", "public"] },
+{ include: ["@<Eigen/src/Geometry/.*>", "private", "<Eigen/Geometry>", "public"] },
+{ include: ["@<Eigen/src/Householder/.*>", "private", "<Eigen/Householder>", "public"] },
+{ include: ["@<Eigen/src/IterativeLinearSolvers/.*>", "private", "<Eigen/IterativeLinearSolvers>", "public"] },
+{ include: ["@<Eigen/src/Jacobi/.*>", "private", "<Eigen/Jacobi>", "public"] },
+{ include: ["@<Eigen/src/LU/.*>", "private", "<Eigen/LU>", "public"] },
+{ include: ["@<Eigen/src/MetisSupport/.*>", "private", "<Eigen/MetisSupport>", "public"] },
+{ include: ["@<Eigen/src/OrderingMethods/.*>", "private", "<Eigen/OrderingMethods>", "public"] },
+{ include: ["@<Eigen/src/PardisoSupport/.*>", "private", "<Eigen/PardisoSupport>", "public"] },
+{ include: ["@<Eigen/src/PaStiXSupport/.*>", "private", "<Eigen/PaStiXSupport>", "public"] },
+{ include: ["@<Eigen/src/QR/.*>", "private", "<Eigen/QR>", "public"] },
+{ include: ["@<Eigen/src/QtAlignedMalloc/.*>", "private", "<Eigen/QtAlignedMalloc>", "public"] },
+{ include: ["@<Eigen/src/Sparse/.*>", "private", "<Eigen/Sparse>", "public"] },
+{ include: ["@<Eigen/src/SparseCholesky/.*>", "private", "<Eigen/SparseCholesky>", "public"] },
+{ include: ["@<Eigen/src/SparseCore/.*>", "private", "<Eigen/SparseCore>", "public"] },
+{ include: ["@<Eigen/src/SparseLU/.*>", "private", "<Eigen/SparseLU>", "public"] },
+{ include: ["@<Eigen/src/SparseQR/.*>", "private", "<Eigen/SparseQR>", "public"] },
+{ include: ["@<Eigen/src/SPQRSupport/.*>", "private", "<Eigen/SPQRSupport>", "public"] },
+{ include: ["@<Eigen/src/src/.*>", "private", "<Eigen/src>", "public"] },
+{ include: ["@<Eigen/src/StdDeque/.*>", "private", "<Eigen/StdDeque>", "public"] },
+{ include: ["@<Eigen/src/StdList/.*>", "private", "<Eigen/StdList>", "public"] },
+{ include: ["@<Eigen/src/StdVector/.*>", "private", "<Eigen/StdVector>", "public"] },
+{ include: ["@<Eigen/src/SuperLUSupport/.*>", "private", "<Eigen/SuperLUSupport>", "public"] },
+{ include: ["@<Eigen/src/SVD/.*>", "private", "<Eigen/SVD>", "public"] },
+{ include: ["@<Eigen/src/UmfPackSupport/.*>", "private", "<Eigen/UmfPackSupport>", "public"] },
+{ include: ["@<opencv2/aruco/.*.hpp>", "private", "<opencv2/aruco.hpp>", "public"] },
+{ include: ["@<opencv2/bgsegm/.*.hpp>", "private", "<opencv2/bgsegm.hpp>", "public"] },
+{ include: ["@<opencv2/bioinspired/.*.hpp>", "private", "<opencv2/bioinspired.hpp>", "public"] },
+{ include: ["@<opencv2/calib3d/.*.hpp>", "private", "<opencv2/calib3d.hpp>", "public"] },
+{ include: ["@<opencv2/ccalib/.*.hpp>", "private", "<opencv2/ccalib.hpp>", "public"] },
+{ include: ["@<opencv2/core_detect/.*.hpp>", "private", "<opencv2/core_detect.hpp>", "public"] },
+{ include: ["@<opencv2/core/.*.hpp>", "private", "<opencv2/core.hpp>", "public"] },
+{ include: ["@<opencv2/dnn/.*.hpp>", "private", "<opencv2/dnn.hpp>", "public"] },
+{ include: ["@<opencv2/dpm/.*.hpp>", "private", "<opencv2/dpm.hpp>", "public"] },
+{ include: ["@<opencv2/face/.*.hpp>", "private", "<opencv2/face.hpp>", "public"] },
+{ include: ["@<opencv2/features2d/.*.hpp>", "private", "<opencv2/features2d.hpp>", "public"] },
+{ include: ["@<opencv2/flann/.*.hpp>", "private", "<opencv2/flann.hpp>", "public"] },
+{ include: ["@<opencv2/freetype/.*.hpp>", "private", "<opencv2/freetype.hpp>", "public"] },
+{ include: ["@<opencv2/fuzzy/.*.hpp>", "private", "<opencv2/fuzzy.hpp>", "public"] },
+{ include: ["@<opencv2/gapi/.*.hpp>", "private", "<opencv2/gapi.hpp>", "public"] },
+{ include: ["@<opencv2/hdf/.*.hpp>", "private", "<opencv2/hdf.hpp>", "public"] },
+{ include: ["@<opencv2/hfs/.*.hpp>", "private", "<opencv2/hfs.hpp>", "public"] },
+{ include: ["@<opencv2/highgui/.*.hpp>", "private", "<opencv2/highgui.hpp>", "public"] },
+{ include: ["@<opencv2/imgcodecs/.*.hpp>", "private", "<opencv2/imgcodecs.hpp>", "public"] },
+{ include: ["@<opencv2/img_hash/.*.hpp>", "private", "<opencv2/img_hash.hpp>", "public"] },
+{ include: ["@<opencv2/imgproc/.*.hpp>", "private", "<opencv2/imgproc.hpp>", "public"] },
+{ include: ["@<opencv2/line_descriptor/.*.hpp>", "private", "<opencv2/line_descriptor.hpp>", "public"] },
+{ include: ["@<opencv2/ml/.*.hpp>", "private", "<opencv2/ml.hpp>", "public"] },
+{ include: ["@<opencv2/objdetect/.*.hpp>", "private", "<opencv2/objdetect.hpp>", "public"] },
+{ include: ["@<opencv2/opencv/.*.hpp>", "private", "<opencv2/opencv.hpp>", "public"] },
+{ include: ["@<opencv2/opencv_modules/.*.hpp>", "private", "<opencv2/opencv_modules.hpp>", "public"] },
+{ include: ["@<opencv2/optflow/.*.hpp>", "private", "<opencv2/optflow.hpp>", "public"] },
+{ include: ["@<opencv2/phase_unwrapping/.*.hpp>", "private", "<opencv2/phase_unwrapping.hpp>", "public"] },
+{ include: ["@<opencv2/photo/.*.hpp>", "private", "<opencv2/photo.hpp>", "public"] },
+{ include: ["@<opencv2/plot/.*.hpp>", "private", "<opencv2/plot.hpp>", "public"] },
+{ include: ["@<opencv2/quality/.*.hpp>", "private", "<opencv2/quality.hpp>", "public"] },
+{ include: ["@<opencv2/rgbd/.*.hpp>", "private", "<opencv2/rgbd.hpp>", "public"] },
+{ include: ["@<opencv2/saliency/.*.hpp>", "private", "<opencv2/saliency.hpp>", "public"] },
+{ include: ["@<opencv2/shape/.*.hpp>", "private", "<opencv2/shape.hpp>", "public"] },
+{ include: ["@<opencv2/stereo/.*.hpp>", "private", "<opencv2/stereo.hpp>", "public"] },
+{ include: ["@<opencv2/stitching/.*.hpp>", "private", "<opencv2/stitching.hpp>", "public"] },
+{ include: ["@<opencv2/structured_light/.*.hpp>", "private", "<opencv2/structured_light.hpp>", "public"] },
+{ include: ["@<opencv2/superres/.*.hpp>", "private", "<opencv2/superres.hpp>", "public"] },
+{ include: ["@<opencv2/surface_matching/.*.hpp>", "private", "<opencv2/surface_matching.hpp>", "public"] },
+{ include: ["@<opencv2/text/.*.hpp>", "private", "<opencv2/text.hpp>", "public"] },
+{ include: ["@<opencv2/tracking/.*.hpp>", "private", "<opencv2/tracking.hpp>", "public"] },
+{ include: ["@<opencv2/video/.*.hpp>", "private", "<opencv2/video.hpp>", "public"] },
+{ include: ["@<opencv2/videoio/.*.hpp>", "private", "<opencv2/videoio.hpp>", "public"] },
+{ include: ["@<opencv2/videostab/.*.hpp>", "private", "<opencv2/videostab.hpp>", "public"] },
+{ include: ["@<opencv2/viz/.*.hpp>", "private", "<opencv2/viz.hpp>", "public"] },
+{ include: ["@<opencv2/xfeatures2d/.*.hpp>", "private", "<opencv2/xfeatures2d.hpp>", "public"] },
+{ include: ["@<opencv2/ximgproc/.*.hpp>", "private", "<opencv2/ximgproc.hpp>", "public"] },
+{ include: ["@<opencv2/xobjdetect/.*.hpp>", "private", "<opencv2/xobjdetect.hpp>", "public"] },
+{ include: ["@<opencv2/xphoto/.*.hpp>", "private", "<opencv2/xphoto.hpp>", "public"] },
+{ include: ["@<pangolin/.*>", "private", "<pangolin/pangolin.h>", "public"] }
+]
diff --git a/iwyu.sh b/iwyu.sh
new file mode 100644
index 0000000000..8ef810fd5f
--- /dev/null
+++ b/iwyu.sh
@@ -0,0 +1,11 @@
+mkdir -p build
+cd build
+
+# create compile_commands.json
+cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
+
+echo "Running include-what-you-use. This can take a very long time (5-15 minutes)."
+iwyu-tool -p . -- -Xiwyu --mapping_file=../iwyu.imp | tee iwyu.out
+
+echo "Now fixing the includes"
+iwyu-fix-includes -m < iwyu.out
diff --git a/src/Converter.cc b/src/Converter.cc
index 00ea8ee597..b0f2872d7b 100644
--- a/src/Converter.cc
+++ b/src/Converter.cc
@@ -19,7 +19,11 @@
 */
 
 
-#include "Converter.h"
+#include "Converter.h"   // IWYU pragma: associated
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <algorithm>
 
 namespace ORB_SLAM2
 {
diff --git a/src/Frame.cc b/src/Frame.cc
index 0e37d49335..1af0526fac 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -18,10 +18,22 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Frame.h"
+#include "Frame.h"   // IWYU pragma: associated
+
+#include <ext/alloc_traits.h>
+#include <ext/new_allocator.h>
+#include <limits.h>
+#include <math.h>
+#include <opencv2/calib3d.hpp>
+#include <thread>
+#include <algorithm>
+#include <memory>
+#include <utility>
+
 #include "Converter.h"
 #include "ORBmatcher.h"
-#include <thread>
+#include "MapPoint.h"
+#include "ORBextractor.h"
 
 namespace ORB_SLAM2
 {
diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc
index e23b86c207..03dffebdb7 100644
--- a/src/FrameDrawer.cc
+++ b/src/FrameDrawer.cc
@@ -18,11 +18,16 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "FrameDrawer.h"
-#include "Tracking.h"
+#include "FrameDrawer.h"   // IWYU pragma: associated
+
+#include <opencv2/imgproc.hpp>
+#include <memory>
+#include <ostream>
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
+#include "Tracking.h"
+#include "Frame.h"
+#include "Map.h"
+#include "MapPoint.h"
 
 #include<mutex>
 
diff --git a/src/Initializer.cc b/src/Initializer.cc
index 6094b8f51b..92c78dc01f 100644
--- a/src/Initializer.cc
+++ b/src/Initializer.cc
@@ -18,12 +18,17 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Initializer.h"
+#include "Initializer.h"   // IWYU pragma: associated
 
-#include "Thirdparty/DBoW2/DUtils/Random.h"
+#include <ext/alloc_traits.h>
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <type_traits>
 
-#include "Optimizer.h"
-#include "ORBmatcher.h"
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+#include "Frame.h"
 
 #include<thread>
 
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 4ef1e78e0f..082ab3a586 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -18,9 +18,21 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "KeyFrame.h"
+#include "KeyFrame.h"   // IWYU pragma: associated
+
+#include <ext/alloc_traits.h>
+#include <math.h>
+#include <algorithm>
+#include <list>
+#include <memory>
+#include <utility>
+
 #include "Converter.h"
-#include "ORBmatcher.h"
+#include "Frame.h"
+#include "KeyFrameDatabase.h"
+#include "Map.h"
+#include "MapPoint.h"
+
 #include<mutex>
 
 namespace ORB_SLAM2
diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc
index 826860cab9..731e6d03b0 100644
--- a/src/KeyFrameDatabase.cc
+++ b/src/KeyFrameDatabase.cc
@@ -18,10 +18,18 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "KeyFrameDatabase.h"
+#include "KeyFrameDatabase.h"   // IWYU pragma: associated
+
+#include <algorithm>
+#include <iosfwd>
+#include <map>
+#include <memory>
+#include <set>
+#include <utility>
 
 #include "KeyFrame.h"
 #include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "Frame.h"
 
 #include<mutex>
 
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index 6c87a6e55a..0c025fbb91 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -18,10 +18,24 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "LocalMapping.h"
+#include "LocalMapping.h"   // IWYU pragma: associated
+
+#include <unistd.h>
+#include <math.h>
+#include <stddef.h>
+#include <algorithm>
+#include <iostream>
+#include <map>
+#include <memory>
+#include <utility>
+#include <vector>
+
 #include "LoopClosing.h"
 #include "ORBmatcher.h"
 #include "Optimizer.h"
+#include "KeyFrame.h"
+#include "Map.h"
+#include "MapPoint.h"
 
 #include<mutex>
 
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 5e317dd420..280cbb405a 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -18,15 +18,27 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "LoopClosing.h"
+#include "LoopClosing.h"   // IWYU pragma: associated
 
-#include "Sim3Solver.h"
+#include <unistd.h>
+#include <ext/alloc_traits.h>
+#include <stddef.h>
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <iostream>
+#include <memory>
 
+#include "Sim3Solver.h"
 #include "Converter.h"
-
 #include "Optimizer.h"
-
 #include "ORBmatcher.h"
+#include "KeyFrame.h"
+#include "KeyFrameDatabase.h"
+#include "LocalMapping.h"
+#include "Map.h"
+#include "MapPoint.h"
+
+namespace DBoW2 { class BowVector; }
 
 #include<mutex>
 #include<thread>
diff --git a/src/Map.cc b/src/Map.cc
index 15fcd86914..87aa7d220a 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -18,7 +18,10 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Map.h"
+#include "Map.h"   // IWYU pragma: associated
+
+#include "KeyFrame.h"
+#include "MapPoint.h"
 
 #include<mutex>
 
diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc
index 4d9990bc8a..a0b411a649 100644
--- a/src/MapDrawer.cc
+++ b/src/MapDrawer.cc
@@ -18,11 +18,18 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "MapDrawer.h"
-#include "MapPoint.h"
-#include "KeyFrame.h"
+#include "MapDrawer.h"   // IWYU pragma: associated
+
 #include <pangolin/pangolin.h>
+#include <GL/glew.h>
+#include <stddef.h>
 #include <mutex>
+#include <set>
+#include <vector>
+
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include "Map.h"
 
 namespace ORB_SLAM2
 {
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 3b2921197b..1ee29acee5 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -18,8 +18,19 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "MapPoint.h"
+#include "MapPoint.h"   // IWYU pragma: associated
+
+#include <limits.h>
+#include <math.h>
+#include <algorithm>
+#include <memory>
+#include <utility>
+#include <vector>
+
 #include "ORBmatcher.h"
+#include "Frame.h"
+#include "KeyFrame.h"
+#include "Map.h"
 
 #include<mutex>
 
diff --git a/src/ORBextractor.cc b/src/ORBextractor.cc
index 14be2efae5..4ca2c16c04 100644
--- a/src/ORBextractor.cc
+++ b/src/ORBextractor.cc
@@ -54,13 +54,20 @@
 */
 
 
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <vector>
+#include "ORBextractor.h"   // IWYU pragma: associated
 
-#include "ORBextractor.h"
+#include <assert.h>
+#include <ext/alloc_traits.h>
+#include <math.h>
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
+#include <vector>
+#include <algorithm>
+#include <cstddef>
+#include <iterator>
+#include <memory>
+#include <type_traits>
+#include <utility>
 
 
 using namespace cv;
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 56bf279d0f..eb05785e4b 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -18,7 +18,7 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "ORBmatcher.h"
+#include "ORBmatcher.h"   // IWYU pragma: associated
 
 #include<limits.h>
 
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 83d9264546..8819c64579 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -18,7 +18,7 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Optimizer.h"
+#include "Optimizer.h"   // IWYU pragma: associated
 
 #include "Thirdparty/g2o/g2o/core/block_solver.h"
 #include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
diff --git a/src/PnPsolver.cc b/src/PnPsolver.cc
index af3511b165..f65b697d8b 100644
--- a/src/PnPsolver.cc
+++ b/src/PnPsolver.cc
@@ -48,15 +48,17 @@
 *   either expressed or implied, of the FreeBSD Project
 */
 
-#include <iostream>
-
-#include "PnPsolver.h"
+#include "PnPsolver.h"   // IWYU pragma: associated
 
+#include <iostream>
 #include <vector>
 #include <cmath>
-#include <opencv2/core/core.hpp>
-#include "Thirdparty/DBoW2/DUtils/Random.h"
 #include <algorithm>
+#include <memory>
+
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+#include "Frame.h"
+#include "MapPoint.h"
 
 using namespace std;
 
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index 4ad44ff0de..cb92505f72 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -19,16 +19,19 @@
 */
 
 
-#include "Sim3Solver.h"
+#include "Sim3Solver.h"   // IWYU pragma: associated
 
+#include <ext/alloc_traits.h>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/core_c.h>
 #include <vector>
 #include <cmath>
-#include <opencv2/core/core.hpp>
+#include <algorithm>
+#include <memory>
 
 #include "KeyFrame.h"
-#include "ORBmatcher.h"
-
 #include "Thirdparty/DBoW2/DUtils/Random.h"
+#include "MapPoint.h"
 
 namespace ORB_SLAM2
 {
diff --git a/src/System.cc b/src/System.cc
index 8df4157095..a5bd20fe2f 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -20,14 +20,32 @@
 
 
 
-#include "System.h"
-#include "Converter.h"
-#include <thread>
+#include "System.h"   // IWYU pragma: associated
+
 #include <pangolin/pangolin.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <thread>
 #include <iomanip>
+#include <algorithm>
+#include <iostream>
+#include <list>
+
+#include "Converter.h"
+#include "Frame.h"
+#include "FrameDrawer.h"
+#include "KeyFrame.h"
+#include "KeyFrameDatabase.h"
+#include "LocalMapping.h"
+#include "LoopClosing.h"
+#include "Map.h"
+#include "MapDrawer.h"
+#include "Tracking.h"
+#include "Viewer.h"
 
 namespace ORB_SLAM2
 {
+class MapPoint;
 
 System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
                const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
@@ -343,18 +361,12 @@ void System::SaveTrajectoryTUM(const string &filename)
     // We need to get first the keyframe pose and then concatenate the relative transformation.
     // Frames not localized (tracking failure) are not saved.
 
-    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
-    // which is true when tracking failed (lbL).
-    list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
-    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
-    list<bool>::iterator lbL = mpTracker->mlbLost.begin();
-    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
-        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
+    for (list<Tracking::TrackedFrame>::iterator iter = mpTracker->tracked_frames.begin(), iter_end = mpTracker->tracked_frames.end(); iter != iter_end; iter++)
     {
-        if(*lbL)
+        if(iter->lost)
             continue;
 
-        KeyFrame* pKF = *lRit;
+        KeyFrame* pKF = iter->reference_keyframe;
 
         cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
 
@@ -367,13 +379,13 @@ void System::SaveTrajectoryTUM(const string &filename)
 
         Trw = Trw*pKF->GetPose()*Two;
 
-        cv::Mat Tcw = (*lit)*Trw;
+        cv::Mat Tcw = iter->relative_frame_pose*Trw;
         cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
         cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
 
         vector<float> q = Converter::toQuaternion(Rwc);
 
-        f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+        f << setprecision(6) << iter->time << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
     }
     f.close();
     cout << endl << "trajectory saved!" << endl;
@@ -440,13 +452,9 @@ void System::SaveTrajectoryKITTI(const string &filename)
     // We need to get first the keyframe pose and then concatenate the relative transformation.
     // Frames not localized (tracking failure) are not saved.
 
-    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
-    // which is true when tracking failed (lbL).
-    list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
-    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
-    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
+    for (list<Tracking::TrackedFrame>::iterator iter = mpTracker->tracked_frames.begin(), iter_end = mpTracker->tracked_frames.end(); iter != iter_end; iter++)
     {
-        ORB_SLAM2::KeyFrame* pKF = *lRit;
+        ORB_SLAM2::KeyFrame* pKF = iter->reference_keyframe;
 
         cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
 
@@ -459,7 +467,7 @@ void System::SaveTrajectoryKITTI(const string &filename)
 
         Trw = Trw*pKF->GetPose()*Two;
 
-        cv::Mat Tcw = (*lit)*Trw;
+        cv::Mat Tcw = iter->relative_frame_pose*Trw;
         cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
         cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 2273b2ce48..58c74db087 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -19,9 +19,10 @@
 */
 
 
-#include "Tracking.h"
+#include "Tracking.h"   // IWYU pragma: associated
 
 #include<opencv2/core/core.hpp>
+#include<opencv2/imgproc/imgproc_c.h>
 #include<opencv2/features2d/features2d.hpp>
 
 #include"ORBmatcher.h"
@@ -33,6 +34,8 @@
 #include"Optimizer.h"
 #include"PnPsolver.h"
 
+#include <unistd.h>
+
 #include<iostream>
 
 #include<mutex>
@@ -488,19 +491,19 @@ void Tracking::Track()
     // Store frame pose information to retrieve the complete camera trajectory afterwards.
     if(!mCurrentFrame.mTcw.empty())
     {
-        cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
-        mlRelativeFramePoses.push_back(Tcr);
-        mlpReferences.push_back(mpReferenceKF);
-        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
-        mlbLost.push_back(mState==LOST);
+        TrackedFrame tracked_frame;
+        tracked_frame.relative_frame_pose = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
+        tracked_frame.reference_keyframe = mpReferenceKF;
+        tracked_frame.time = mCurrentFrame.mTimeStamp;
+        tracked_frame.lost = (mState == LOST);
+        tracked_frames.push_back(tracked_frame);
     }
     else
     {
         // This can happen if tracking is lost
-        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
-        mlpReferences.push_back(mlpReferences.back());
-        mlFrameTimes.push_back(mlFrameTimes.back());
-        mlbLost.push_back(mState==LOST);
+        TrackedFrame tracked_frame = tracked_frames.back();
+        tracked_frame.lost = (mState == LOST);
+        tracked_frames.push_back(tracked_frame);
     }
 
 }
@@ -802,7 +805,7 @@ void Tracking::UpdateLastFrame()
 {
     // Update pose according to reference keyframe
     KeyFrame* pRef = mLastFrame.mpReferenceKF;
-    cv::Mat Tlr = mlRelativeFramePoses.back();
+    cv::Mat Tlr = tracked_frames.back().relative_frame_pose;
 
     mLastFrame.SetPose(Tlr*pRef->GetPose());
 
@@ -1540,10 +1543,7 @@ void Tracking::Reset()
         mpInitializer = static_cast<Initializer*>(NULL);
     }
 
-    mlRelativeFramePoses.clear();
-    mlpReferences.clear();
-    mlFrameTimes.clear();
-    mlbLost.clear();
+    tracked_frames.clear();
 
     if(mpViewer)
         mpViewer->Release();
diff --git a/src/Viewer.cc b/src/Viewer.cc
index dec3204f53..9fc13e6505 100644
--- a/src/Viewer.cc
+++ b/src/Viewer.cc
@@ -18,11 +18,19 @@
 * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include "Viewer.h"
-#include <pangolin/pangolin.h>
+#include "Viewer.h"   // IWYU pragma: associated
 
+#include <pangolin/pangolin.h>
+#include <unistd.h>
+#include <GL/glew.h>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
 #include <mutex>
 
+#include "FrameDrawer.h"
+#include "MapDrawer.h"
+#include "System.h"
+
 namespace ORB_SLAM2
 {