diff --git a/CMakeLists.txt b/CMakeLists.txt
index b7c53a0..b9bc989 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,10 +63,11 @@ add_library(omni_slam_eval_lib
   src/data/frame.cc
   src/data/feature.cc
   src/data/landmark.cc
-  src/feature/tracker.cc
+  src/feature/lk_tracker.cc
   src/feature/detector.cc
   src/feature/matcher.cc
   src/reconstruction/triangulator.cc
+  src/odometry/pose_estimator.cc
   src/odometry/pnp.cc
   src/odometry/five_point.cc
   src/optimization/bundle_adjuster.cc
diff --git a/src/feature/tracker.cc b/src/feature/lk_tracker.cc
similarity index 90%
rename from src/feature/tracker.cc
rename to src/feature/lk_tracker.cc
index 421bdb7..2000403 100644
--- a/src/feature/tracker.cc
+++ b/src/feature/lk_tracker.cc
@@ -1,4 +1,4 @@
-#include "tracker.h"
+#include "lk_tracker.h"
 #include <cmath>
 #include "util/tf_util.h"
 
@@ -7,7 +7,7 @@ namespace omni_slam
 namespace feature
 {
 
-Tracker::Tracker(const int window_size, const int num_scales, const float delta_pix_err_thresh, const float err_thresh, const int term_count, const double term_eps)
+LKTracker::LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh, const float err_thresh, const int term_count, const double term_eps)
     : windowSize_(window_size / pow(2, num_scales), window_size / pow(2, num_scales)),
     numScales_(num_scales),
     errThresh_(err_thresh),
@@ -17,12 +17,12 @@ Tracker::Tracker(const int window_size, const int num_scales, const float delta_
 {
 }
 
-void Tracker::Init(data::Frame &init_frame)
+void LKTracker::Init(data::Frame &init_frame)
 {
     prevFrame_ = &init_frame;
 }
 
-int Tracker::Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors)
+int LKTracker::Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors)
 {
     if (prevFrame_ == nullptr)
     {
diff --git a/src/feature/lk_tracker.h b/src/feature/lk_tracker.h
new file mode 100644
index 0000000..b60a7b5
--- /dev/null
+++ b/src/feature/lk_tracker.h
@@ -0,0 +1,34 @@
+#ifndef _LK_TRACKER_H_
+#define _LK_TRACKER_H_
+
+#include "tracker.h"
+#include <opencv2/opencv.hpp>
+#include "data/frame.h"
+#include "data/landmark.h"
+#include "odometry/five_point.h"
+#include <vector>
+
+namespace omni_slam
+{
+namespace feature
+{
+
+class LKTracker : public Tracker
+{
+public:
+    LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int term_count = 50, const double term_eps = 0.01);
+    void Init(data::Frame &init_frame);
+    int Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors);
+
+private:
+    cv::TermCriteria termCrit_;
+    const cv::Size windowSize_;
+    const int numScales_;
+    const float errThresh_;
+    const float deltaPixErrThresh_;
+    data::Frame *prevFrame_;
+};
+
+}
+}
+#endif /* _LK_TRACKER_H_ */
diff --git a/src/feature/tracker.h b/src/feature/tracker.h
index f9ad316..c2d109f 100644
--- a/src/feature/tracker.h
+++ b/src/feature/tracker.h
@@ -1,11 +1,8 @@
 #ifndef _TRACKER_H_
 #define _TRACKER_H_
 
-#include <opencv2/opencv.hpp>
 #include "data/frame.h"
 #include "data/landmark.h"
-#include "odometry/five_point.h"
-#include <vector>
 
 namespace omni_slam
 {
@@ -15,19 +12,11 @@ namespace feature
 class Tracker
 {
 public:
-    Tracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int term_count = 50, const double term_eps = 0.01);
-    void Init(data::Frame &init_frame);
-    int Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors);
-
-private:
-    cv::TermCriteria termCrit_;
-    const cv::Size windowSize_;
-    const int numScales_;
-    const float errThresh_;
-    const float deltaPixErrThresh_;
-    data::Frame *prevFrame_;
+    virtual void Init(data::Frame &init_frame) = 0;
+    virtual int Track(std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, std::vector<double> &errors) = 0;
 };
 
 }
 }
+
 #endif /* _TRACKER_H_ */
diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc
index ee465cc..2e6bd84 100644
--- a/src/module/odometry_module.cc
+++ b/src/module/odometry_module.cc
@@ -7,24 +7,24 @@ namespace omni_slam
 namespace module
 {
 
-OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &pnp, std::unique_ptr<optimization::BundleAdjuster> &bundle_adjuster)
-    : pnp_(std::move(pnp)),
+OdometryModule::OdometryModule(std::unique_ptr<odometry::PoseEstimator> &pose_estimator, std::unique_ptr<optimization::BundleAdjuster> &bundle_adjuster)
+    : poseEstimator_(std::move(pose_estimator)),
     bundleAdjuster_(std::move(bundle_adjuster))
 {
 }
 
-OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &&pnp, std::unique_ptr<optimization::BundleAdjuster> &&bundle_adjuster)
-    : OdometryModule(pnp, bundle_adjuster)
+OdometryModule::OdometryModule(std::unique_ptr<odometry::PoseEstimator> &&pose_estimator, std::unique_ptr<optimization::BundleAdjuster> &&bundle_adjuster)
+    : OdometryModule(pose_estimator, bundle_adjuster)
 {
 }
 
-void OdometryModule::Update(std::vector<data::Landmark> &landmarks, data::Frame &frame)
+void OdometryModule::Update(std::vector<data::Landmark> &landmarks, std::vector<std::unique_ptr<data::Frame>> &frames)
 {
     if (landmarks.size() == 0)
     {
         return;
     }
-    pnp_->Compute(landmarks, frame);
+    poseEstimator_->Compute(landmarks, *frames.back(), **next(frames.rbegin()));
 }
 
 void OdometryModule::BundleAdjust(std::vector<data::Landmark> &landmarks)
diff --git a/src/module/odometry_module.h b/src/module/odometry_module.h
index 93d7189..1e1248f 100644
--- a/src/module/odometry_module.h
+++ b/src/module/odometry_module.h
@@ -5,7 +5,7 @@
 #include <set>
 #include <memory>
 
-#include "odometry/pnp.h"
+#include "odometry/pose_estimator.h"
 #include "optimization/bundle_adjuster.h"
 #include "data/landmark.h"
 
@@ -26,16 +26,16 @@ class OdometryModule
     {
     };
 
-    OdometryModule(std::unique_ptr<odometry::PNP> &pnp, std::unique_ptr<optimization::BundleAdjuster> &bundle_adjuster);
-    OdometryModule(std::unique_ptr<odometry::PNP> &&pnp, std::unique_ptr<optimization::BundleAdjuster> &&bundle_adjuster);
+    OdometryModule(std::unique_ptr<odometry::PoseEstimator> &pose_estimator, std::unique_ptr<optimization::BundleAdjuster> &bundle_adjuster);
+    OdometryModule(std::unique_ptr<odometry::PoseEstimator> &&pose_estimator, std::unique_ptr<optimization::BundleAdjuster> &&bundle_adjuster);
 
-    void Update(std::vector<data::Landmark> &landmarks, data::Frame &frame);
+    void Update(std::vector<data::Landmark> &landmarks, std::vector<std::unique_ptr<data::Frame>> &frames);
     void BundleAdjust(std::vector<data::Landmark> &landmarks);
 
     Stats& GetStats();
 
 private:
-    std::shared_ptr<odometry::PNP> pnp_;
+    std::shared_ptr<odometry::PoseEstimator> poseEstimator_;
     std::shared_ptr<optimization::BundleAdjuster> bundleAdjuster_;
 
     Stats stats_;
diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc
index 49cf6ac..838a8b6 100644
--- a/src/module/tracking_module.cc
+++ b/src/module/tracking_module.cc
@@ -46,7 +46,7 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
     {
         Matrix3d E;
         std::vector<int> inlierIndices;
-        fivePointChecker_->Compute(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices);
+        fivePointChecker_->ComputeE(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices);
         std::unordered_set<int> inlierSet(inlierIndices.begin(), inlierIndices.end());
         for (int i = 0; i < landmarks_.size(); i++)
         {
diff --git a/src/odometry/five_point.cc b/src/odometry/five_point.cc
index e682939..595ed3d 100644
--- a/src/odometry/five_point.cc
+++ b/src/odometry/five_point.cc
@@ -1,21 +1,113 @@
 #include "five_point.h"
 
 #include "util/math_util.h"
+#include "util/tf_util.h"
 #include <set>
 #include <random>
+#include <ceres/ceres.h>
+#include "optimization/reprojection_error.h"
+#include "camera/double_sphere.h"
+#include "camera/perspective.h"
 
 namespace omni_slam
 {
 namespace odometry
 {
 
-FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold)
+FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int num_ceres_threads)
     : ransacIterations_(ransac_iterations),
-    epipolarThreshold_(epipolar_threshold)
+    epipolarThreshold_(epipolar_threshold),
+    numCeresThreads_(num_ceres_threads)
 {
 }
 
-int FivePoint::Compute(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector<int> &inlier_indices) const
+int FivePoint::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector<int> &inlier_indices) const
+{
+    Matrix3d E;
+    int inliers = ComputeE(landmarks, prev_frame, cur_frame, E, inlier_indices);
+    std::vector<Matrix3d> rs;
+    std::vector<Vector3d> ts;
+    EssentialToPoses(E, rs, ts);
+    Matrix<double, 3, 4> I = util::TFUtil::IdentityPoseMatrix<double>();
+    int bestPoints = 0;
+    Matrix<double, 3, 4> bestPose;
+    std::vector<Vector3d> bestXs;
+    std::vector<const data::Feature*> bestFeats;
+    std::vector<int> bestIds;
+    for (int i = 0; i < rs.size(); i++)
+    {
+        Matrix<double, 3, 4> P;
+        P.block<3, 3>(0, 0) = rs[i];
+        P.block<3, 1>(0, 3) = ts[i];
+        int goodPoints = 0;
+        std::vector<Vector3d> xs;
+        std::vector<const data::Feature*> feats;
+        std::vector<int> ids;
+        for (int inx : inlier_indices)
+        {
+            const data::Landmark &landmark = landmarks[inx];
+            const data::Feature *feat1 = landmark.GetObservationByFrameID(prev_frame.GetID());
+            const data::Feature *feat2 = landmark.GetObservationByFrameID(cur_frame.GetID());
+            Vector3d X = TriangulateDLT(feat1->GetBearing().normalized(), feat2->GetBearing().normalized(), I, P);
+
+            Vector2d pix;
+            if (cur_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(X), pix) && prev_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(P, X)), pix))
+            {
+                goodPoints++;
+                if (prev_frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr)
+                {
+                    xs.push_back(landmark.GetEstimatedPosition());
+                    feats.push_back(feat2);
+                    ids.push_back(landmark.GetID());
+                }
+                else if (!prev_frame.HasStereoImage() && landmark.HasEstimatedPosition())
+                {
+                    xs.push_back(landmark.GetEstimatedPosition());
+                    feats.push_back(feat2);
+                    ids.push_back(landmark.GetID());
+                }
+                else if (!prev_frame.HasStereoImage() && landmark.HasGroundTruth())
+                {
+                    xs.push_back(landmark.GetGroundTruth());
+                    feats.push_back(feat2);
+                    ids.push_back(landmark.GetID());
+                }
+            }
+        }
+        if (goodPoints > bestPoints)
+        {
+            bestPoints = goodPoints;
+            bestPose = P;
+            bestXs = xs;
+            bestFeats = feats;
+            bestIds = ids;
+        }
+    }
+    Matrix<double, 3, 4> estPose;
+    Vector3d t;
+    if (prev_frame.HasEstimatedPose())
+    {
+        estPose = util::TFUtil::CombineTransforms(bestPose, prev_frame.GetEstimatedInversePose());
+        t = prev_frame.GetEstimatedInversePose().block<3, 1>(0, 3);
+    }
+    else if (prev_frame.HasPose())
+    {
+        estPose = util::TFUtil::CombineTransforms(bestPose, prev_frame.GetInversePose());
+        t = prev_frame.GetInversePose().block<3, 1>(0, 3);
+    }
+    else
+    {
+        estPose = bestPose;
+        t << 0, 0, 0;
+    }
+    ComputeTranslation(bestXs, bestFeats, util::TFUtil::GetRotationFromPoseMatrix(estPose), t);
+    estPose.block<3, 1>(0, 3) = t;
+    cur_frame.SetEstimatedInversePose(estPose, bestIds);
+
+    return inliers;
+}
+
+int FivePoint::ComputeE(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector<int> &inlier_indices) const
 {
     std::vector<Vector3d> x1;
     std::vector<Vector3d> x2;
@@ -48,12 +140,6 @@ int FivePoint::Compute(const std::vector<data::Landmark> &landmarks, const data:
     return inliers;
 }
 
-int FivePoint::Compute(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E) const
-{
-    std::vector<int> temp;
-    return Compute(landmarks, frame1, frame2, E, temp);
-}
-
 int FivePoint::RANSAC(const std::vector<Vector3d> &x1, const std::vector<Vector3d> &x2, Matrix3d &E) const
 {
     int maxInliers = 0;
@@ -185,6 +271,99 @@ void FivePoint::FivePointRelativePose(const std::vector<Vector3d> &x1, const std
     }
 }
 
+void FivePoint::EssentialToPoses(const Matrix3d &E, std::vector<Matrix3d> &rs, std::vector<Vector3d> &ts) const
+{
+    Eigen::JacobiSVD<Matrix3d> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    Matrix3d U = USV.matrixU();
+    Matrix3d Vt = USV.matrixV().transpose();
+
+    if (U.determinant() < 0)
+    {
+        U.col(2) *= -1;
+    }
+    if (Vt.determinant() < 0)
+    {
+        Vt.row(2) *= -1;
+    }
+    Matrix3d W;
+    W << 0, -1, 0, 1, 0, 0, 0, 0, 1;
+
+    Matrix3d UWVt = U * W * Vt;
+    Matrix3d UWtVt = U * W.transpose() * Vt;
+
+    rs.clear();
+    rs.reserve(4);
+    ts.clear();
+    ts.reserve(4);
+    rs.emplace_back(UWVt);
+    ts.emplace_back(-UWVt.transpose() * U.col(2));
+    rs.emplace_back(UWtVt);
+    ts.emplace_back(-UWtVt.transpose() * -U.col(2));
+    rs.emplace_back(UWVt);
+    ts.emplace_back(-UWVt.transpose() * -U.col(2));
+    rs.emplace_back(UWtVt);
+    ts.emplace_back(-UWtVt.transpose() * U.col(2));
+}
+
+void FivePoint::ComputeTranslation(const std::vector<Vector3d> &xs, const std::vector<const data::Feature*> &ys, const Matrix3d &R, Vector3d &t) const
+{
+    ceres::Problem problem;
+    ceres::LossFunction *loss_function = new ceres::HuberLoss(0.001);
+    std::vector<double> landmarks;
+    landmarks.reserve(3 * xs.size());
+    Quaterniond quat(R);
+    quat.normalize();
+    std::vector<double> quatData(quat.coeffs().data(), quat.coeffs().data() + 4);
+    std::vector<double> tData(t.data(), t.data() + 3);
+    for (int i = 0; i < xs.size(); i++)
+    {
+        const Vector3d &x = xs[i];
+        landmarks.push_back(x(0));
+        landmarks.push_back(x(1));
+        landmarks.push_back(x(2));
+        ceres::CostFunction *cost_function = nullptr;
+        if (ys[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective)
+        {
+            cost_function = optimization::ReprojectionError<camera::Perspective>::Create(*ys[i]);
+        }
+        else if (ys[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere)
+        {
+            cost_function = optimization::ReprojectionError<camera::DoubleSphere>::Create(*ys[i]);
+        }
+        if (cost_function != nullptr)
+        {
+            problem.AddResidualBlock(cost_function, loss_function, &quatData[0], &tData[0], &landmarks[landmarks.size() - 3]);
+            problem.SetParameterBlockConstant(&landmarks[landmarks.size() - 3]);
+            problem.SetParameterBlockConstant(&quatData[0]);
+        }
+    }
+    ceres::Solver::Options options;
+    options.max_num_iterations = 100;
+    options.linear_solver_type = ceres::DENSE_SCHUR;
+    options.function_tolerance = 1e-6;
+    options.gradient_tolerance = 1e-6;
+    options.num_threads = numCeresThreads_;
+    ceres::Solver::Summary summary;
+    ceres::Solve(options, &problem, &summary);
+    if (!summary.IsSolutionUsable())
+    {
+        return;
+    }
+    const Vector3d tRes = Map<const Vector3d>(&tData[0]);
+    t = tRes;
+}
+
+Vector3d FivePoint::TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix<double, 3, 4> &pose1, const Matrix<double, 3, 4> &pose2) const
+{
+    Matrix4d design;
+    design.row(0) = x1(0) * pose1.row(2) - x1(2) * pose1.row(0);
+    design.row(1) = x1(1) * pose1.row(2) - x1(2) * pose1.row(1);
+    design.row(2) = x2(0) * pose2.row(2) - x2(2) * pose2.row(0);
+    design.row(3) = x2(1) * pose2.row(2) - x2(2) * pose2.row(1);
+
+    Eigen::JacobiSVD<Matrix4d> svd(design, Eigen::ComputeFullV);
+    return svd.matrixV().col(3).hnormalized();
+}
 
 }
 }
diff --git a/src/odometry/five_point.h b/src/odometry/five_point.h
index 86f1532..7137b7a 100644
--- a/src/odometry/five_point.h
+++ b/src/odometry/five_point.h
@@ -1,6 +1,7 @@
 #ifndef _FIVE_POINT_H_
 #define _FIVE_POINT_H_
 
+#include "pose_estimator.h"
 #include <Eigen/Dense>
 #include "data/landmark.h"
 
@@ -11,22 +12,25 @@ namespace omni_slam
 namespace odometry
 {
 
-class FivePoint
+class FivePoint : public PoseEstimator
 {
 public:
-    FivePoint(int ransac_iterations, double epipolar_threshold);
+    FivePoint(int ransac_iterations, double epipolar_threshold, int num_ceres_threads = 1);
 
-    int Compute(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector<int> &inlier_indices) const;
-    int Compute(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E) const;
+    int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector<int> &inlier_indices) const;
+    int ComputeE(const std::vector<data::Landmark> &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector<int> &inlier_indices) const;
 
 private:
     int RANSAC(const std::vector<Vector3d> &x1, const std::vector<Vector3d> &x2, Matrix3d &E) const;
     void FivePointRelativePose(const std::vector<Vector3d> &x1, const std::vector<Vector3d> &x2, std::vector<Matrix3d> &Es) const;
     std::vector<int> GetInlierIndices(const std::vector<Vector3d> &x1, const std::vector<Vector3d> &x2, const Matrix3d &E) const;
+    void EssentialToPoses(const Matrix3d &E, std::vector<Matrix3d> &rs, std::vector<Vector3d> &ts) const;
+    void ComputeTranslation(const std::vector<Vector3d> &xs, const std::vector<const data::Feature*> &ys, const Matrix3d &R, Vector3d &t) const;
+    Vector3d TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix<double, 3, 4> &pose1, const Matrix<double, 3, 4> &pose2) const;
 
     int ransacIterations_;
     double epipolarThreshold_;
-
+    int numCeresThreads_;
 };
 
 }
diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc
index e5968bf..e965ad4 100644
--- a/src/odometry/pnp.cc
+++ b/src/odometry/pnp.cc
@@ -23,7 +23,7 @@ PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_th
 {
 }
 
-int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &frame, std::vector<int> &inlier_indices) const
+int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector<int> &inlier_indices) const
 {
     std::vector<Vector3d> xs;
     std::vector<Vector2d> yns;
@@ -34,17 +34,17 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
     int i = 0;
     for (const data::Landmark &landmark : landmarks)
     {
-        if (landmark.IsObservedInFrame(frame.GetID()))
+        if (landmark.IsObservedInFrame(cur_frame.GetID()))
         {
-            if (frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr)
+            if (cur_frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr)
             {
                 xs.push_back(landmark.GetEstimatedPosition());
             }
-            else if (!frame.HasStereoImage() && landmark.HasEstimatedPosition())
+            else if (!cur_frame.HasStereoImage() && landmark.HasEstimatedPosition())
             {
                 xs.push_back(landmark.GetEstimatedPosition());
             }
-            else if (!frame.HasStereoImage() && landmark.HasGroundTruth())
+            else if (!cur_frame.HasStereoImage() && landmark.HasGroundTruth())
             {
                 xs.push_back(landmark.GetGroundTruth());
             }
@@ -53,7 +53,7 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
                 i++;
                 continue;
             }
-            const data::Feature *feat = landmark.GetObservationByFrameID(frame.GetID());
+            const data::Feature *feat = landmark.GetObservationByFrameID(cur_frame.GetID());
             Vector2d pix;
             pix << feat->GetKeypoint().pt.x, feat->GetKeypoint().pt.y;
             yns.push_back(pix);
@@ -69,8 +69,8 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
         return 0;
     }
     Matrix<double, 3, 4> pose;
-    int inliers = RANSAC(xs, ys, yns, frame.GetCameraModel(), pose);
-    std::vector<int> indices = GetInlierIndices(xs, yns, pose, frame.GetCameraModel());
+    int inliers = RANSAC(xs, ys, yns, cur_frame.GetCameraModel(), pose);
+    std::vector<int> indices = GetInlierIndices(xs, yns, pose, cur_frame.GetCameraModel());
     if (inliers > 3)
     {
         Refine(xs, features, indices, pose);
@@ -84,16 +84,10 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
         inlierIds.push_back(indexToId[inx]);
         inlier_indices.push_back(indexToLandmarkIndex[inx]);
     }
-    frame.SetEstimatedInversePose(pose, inlierIds);
+    cur_frame.SetEstimatedInversePose(pose, inlierIds);
     return inliers;
 }
 
-int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &frame) const
-{
-    std::vector<int> temp;
-    return Compute(landmarks, frame, temp);
-}
-
 int PNP::RANSAC(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys, const std::vector<Vector2d> &yns, const camera::CameraModel<> &camera_model, Matrix<double, 3, 4> &pose) const
 {
     int maxInliers = 0;
@@ -137,7 +131,7 @@ bool PNP::Refine(const std::vector<Vector3d> &xs, const std::vector<const data::
     ceres::LossFunction *loss_function = nullptr;
     std::vector<double> landmarks;
     landmarks.reserve(3 * indices.size());
-    Quaterniond quat(pose.block<3, 3>(0, 0));
+    Quaterniond quat(util::TFUtil::GetRotationFromPoseMatrix(pose));
     quat.normalize();
     Vector3d t(pose.block<3, 1>(0, 3));
     std::vector<double> quatData(quat.coeffs().data(), quat.coeffs().data() + 4);
diff --git a/src/odometry/pnp.h b/src/odometry/pnp.h
index 4c3f2aa..939fba4 100644
--- a/src/odometry/pnp.h
+++ b/src/odometry/pnp.h
@@ -1,6 +1,7 @@
 #ifndef _PNP_H_
 #define _PNP_H_
 
+#include "pose_estimator.h"
 #include <Eigen/Dense>
 #include "data/landmark.h"
 #include "data/frame.h"
@@ -14,12 +15,11 @@ namespace omni_slam
 namespace odometry
 {
 
-class PNP
+class PNP : public PoseEstimator
 {
 public:
     PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads = 1);
-    int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &frame, std::vector<int> &inlier_indices) const;
-    int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &frame) const;
+    int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector<int> &inlier_indices) const;
 
 private:
     int RANSAC(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys, const std::vector<Vector2d> &yns, const camera::CameraModel<> &camera_model, Matrix<double, 3, 4> &pose) const;
diff --git a/src/odometry/pose_estimator.cc b/src/odometry/pose_estimator.cc
new file mode 100644
index 0000000..a5c87d7
--- /dev/null
+++ b/src/odometry/pose_estimator.cc
@@ -0,0 +1,15 @@
+#include "pose_estimator.h"
+
+namespace omni_slam
+{
+namespace odometry
+{
+
+int PoseEstimator::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame) const
+{
+    std::vector<int> temp;
+    return Compute(landmarks, cur_frame, prev_frame, temp);
+}
+
+}
+}
diff --git a/src/odometry/pose_estimator.h b/src/odometry/pose_estimator.h
new file mode 100644
index 0000000..339589d
--- /dev/null
+++ b/src/odometry/pose_estimator.h
@@ -0,0 +1,22 @@
+#ifndef _POSE_ESTIMATOR_H_
+#define _POSE_ESTIMATOR_H_
+
+#include <vector>
+#include "data/landmark.h"
+
+namespace omni_slam
+{
+namespace odometry
+{
+
+class PoseEstimator
+{
+public:
+    virtual int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector<int> &inlier_indices) const = 0;
+    int Compute(const std::vector<data::Landmark> &landmarks, data::Frame &cur_frame, data::Frame &prev_frame) const;
+};
+
+}
+}
+
+#endif /* _POSE_ESTIMATOR_H_ */
diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc
index 3927fd6..7813aab 100644
--- a/src/ros/odometry_eval.cc
+++ b/src/ros/odometry_eval.cc
@@ -1,6 +1,7 @@
 #include "odometry_eval.h"
 
 #include "odometry/pnp.h"
+#include "odometry/five_point.h"
 #include "optimization/bundle_adjuster.h"
 #include "module/tracking_module.h"
 
@@ -25,6 +26,9 @@ OdometryEval<Stereo>::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod
     bool logCeres;
     int numCeresThreads;
 
+    double fivePointThreshold;
+    int fivePointRansacIterations;
+
     this->nhp_.param("output_frame", cameraFrame_, std::string("map"));
     this->nhp_.param("pnp_inlier_threshold", reprojThresh, 10.);
     this->nhp_.param("pnp_iterations", iterations, 1000);
@@ -33,10 +37,14 @@ OdometryEval<Stereo>::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod
     this->nhp_.param("bundle_adjustment_logging", logCeres, false);
     this->nhp_.param("bundle_adjustment_num_threads", numCeresThreads, 1);
 
-    unique_ptr<odometry::PNP> pnp(new odometry::PNP(iterations, reprojThresh, numCeresThreads));
+    this->nhp_.param("tracker_checker_epipolar_threshold", fivePointThreshold, 0.01745240643);
+    this->nhp_.param("tracker_checker_iterations", fivePointRansacIterations, 1000);
+
+    unique_ptr<odometry::PoseEstimator> poseEstimator(new odometry::PNP(iterations, reprojThresh, numCeresThreads));
+    //unique_ptr<odometry::PoseEstimator> poseEstimator(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, numCeresThreads));
     unique_ptr<optimization::BundleAdjuster> bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres));
 
-    odometryModule_.reset(new module::OdometryModule(pnp, bundleAdjuster));
+    odometryModule_.reset(new module::OdometryModule(poseEstimator, bundleAdjuster));
 }
 
 template <bool Stereo>
@@ -67,7 +75,7 @@ template <bool Stereo>
 void OdometryEval<Stereo>::ProcessFrame(unique_ptr<data::Frame> &&frame)
 {
     this->trackingModule_->Update(frame);
-    odometryModule_->Update(this->trackingModule_->GetLandmarks(), *this->trackingModule_->GetFrames().back());
+    odometryModule_->Update(this->trackingModule_->GetLandmarks(), this->trackingModule_->GetFrames());
     this->trackingModule_->Redetect();
 
     this->visualized_ = false;
diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc
index 54d5a37..3316780 100644
--- a/src/ros/slam_eval.cc
+++ b/src/ros/slam_eval.cc
@@ -31,7 +31,7 @@ void SLAMEval::InitPublishers()
 void SLAMEval::ProcessFrame(unique_ptr<data::Frame> &&frame)
 {
     trackingModule_->Update(frame);
-    odometryModule_->Update(trackingModule_->GetLandmarks(), *trackingModule_->GetFrames().back());
+    odometryModule_->Update(trackingModule_->GetLandmarks(), trackingModule_->GetFrames());
     reconstructionModule_->Update(trackingModule_->GetLandmarks());
     if (baSlidingWindow_ > 0 && baSlidingInterval_ > 0 && (frameNum_ + 1) % baSlidingInterval_ == 0)
     {
diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc
index f08a808..bcd1198 100644
--- a/src/ros/tracking_eval.cc
+++ b/src/ros/tracking_eval.cc
@@ -1,6 +1,6 @@
 #include "tracking_eval.h"
 
-#include "feature/tracker.h"
+#include "feature/lk_tracker.h"
 #include "feature/detector.h"
 #include "odometry/five_point.h"
 
@@ -47,7 +47,7 @@ TrackingEval<Stereo>::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod
         ROS_ERROR("Invalid feature detector specified");
     }
 
-    unique_ptr<feature::Tracker> tracker(new feature::Tracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh));
+    unique_ptr<feature::Tracker> tracker(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh));
 
     unique_ptr<odometry::FivePoint> checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold));
 
diff --git a/src/util/tf_util.h b/src/util/tf_util.h
index 449f228..62fc028 100644
--- a/src/util/tf_util.h
+++ b/src/util/tf_util.h
@@ -67,6 +67,18 @@ inline Matrix<T, 3, 4> QuaternionTranslationToPoseMatrix(Quaternion<T> quat, Mat
     return pose;
 }
 
+template <typename T>
+inline Matrix<T, 3, 4> CombineTransforms(Matrix<T, 3, 4> pose1, Matrix<T, 3, 4> pose2)
+{
+    Matrix<T, 4, 4> pose1H = Matrix<T, 4, 4>::Zero();
+    Matrix<T, 4, 4> pose2H = Matrix<T, 4, 4>::Zero();
+    pose1H.template block<3, 4>(0, 0) = pose1;
+    pose1H(3, 3) = 1;
+    pose2H.template block<3, 4>(0, 0) = pose2;
+    pose2H(3, 3) = 1;
+    return (pose1H * pose2H).template block<3, 4>(0, 0);
+}
+
 template <typename T>
 inline Matrix<T, 3, 4> GetRelativeTransform(Matrix<T, 3, 4> pose1, Matrix<T, 3, 4> pose2)
 {
@@ -80,8 +92,8 @@ template <typename T>
 inline Matrix<T, 3, 3> GetEssentialMatrixFromPoses(Matrix<T, 3, 4> pose1, Matrix<T, 3, 4> pose2)
 {
     Matrix<T, 3, 4> rel = GetRelativeTransform(pose1, pose2);
-    Vector3d t = -GetRotationFromPoseMatrix(rel).transpose() * rel.template block<3, 1>(0, 3);
-    Matrix3d tx;
+    Matrix<T, 3, 1> t = -GetRotationFromPoseMatrix(rel).transpose() * rel.template block<3, 1>(0, 3);
+    Matrix<T, 3, 3> tx;
     tx << 0, -t(2), t(1),
        t(2), 0, -t(0),
        -t(1), t(0), 0;