diff --git a/src/data/frame.cc b/src/data/frame.cc
index 2e35b63..495386e 100644
--- a/src/data/frame.cc
+++ b/src/data/frame.cc
@@ -22,6 +22,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix
     hasPose_ = true;
     hasDepth_ = true;
     hasStereo_ = true;
+    poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
@@ -35,6 +36,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo
     hasPose_ = false;
     hasDepth_ = false;
     hasStereo_ = true;
+    pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
@@ -47,6 +49,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraMo
     hasPose_ = false;
     hasDepth_ = true;
     hasStereo_ = false;
+    pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
@@ -61,6 +64,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix
     hasPose_ = false;
     hasDepth_ = true;
     hasStereo_ = true;
+    pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model)
@@ -74,6 +78,7 @@ Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::C
     hasPose_ = true;
     hasDepth_ = false;
     hasStereo_ = false;
+    poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4>  &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
@@ -89,6 +94,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4>  &pose,
     hasPose_ = true;
     hasDepth_ = false;
     hasStereo_ = true;
+    poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
@@ -103,6 +109,7 @@ Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, cv::Mat &depth_image,
     hasPose_ = true;
     hasDepth_ = true;
     hasStereo_ = false;
+    poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
@@ -113,6 +120,7 @@ Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
     hasPose_ = false;
     hasDepth_ = false;
     hasStereo_ = false;
+    pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
 }
 
 Frame::Frame(const Frame &other)
diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc
index 94a1520..ee465cc 100644
--- a/src/module/odometry_module.cc
+++ b/src/module/odometry_module.cc
@@ -22,10 +22,6 @@ void OdometryModule::Update(std::vector<data::Landmark> &landmarks, data::Frame
 {
     if (landmarks.size() == 0)
     {
-        if (frame.HasPose())
-        {
-            frame.SetEstimatedPose(frame.GetPose());
-        }
         return;
     }
     pnp_->Compute(landmarks, frame);
diff --git a/src/module/stereo_module.cc b/src/module/stereo_module.cc
index d3b63b8..d950d26 100644
--- a/src/module/stereo_module.cc
+++ b/src/module/stereo_module.cc
@@ -35,8 +35,15 @@ void StereoModule::Update(data::Frame &frame, std::vector<data::Landmark> &landm
         {
             Matrix<double, 3, 4> framePose = frame.HasEstimatedPose() ? frame.GetEstimatedPose() : frame.GetPose();
             double depth = (landmark.GetEstimatedPosition() - framePose.block<3, 1>(0, 3)).norm();
-            double depthGnd = (landmark.GetGroundTruth() - frame.GetPose().block<3, 1>(0, 3)).norm();
-            visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, depthGnd);
+            if (frame.HasPose() && landmark.HasGroundTruth())
+            {
+                double depthGnd = (landmark.GetGroundTruth() - frame.GetPose().block<3, 1>(0, 3)).norm();
+                visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, std::abs(depth - depthGnd) / depthGnd);
+            }
+            else
+            {
+                visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, 0);
+            }
         }
     }
     frameNum_++;
@@ -58,9 +65,8 @@ void StereoModule::Visualization::Init(cv::Size img_size)
     curDepth_ = cv::Mat::zeros(img_size, CV_8UC3);
 }
 
-void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double depthGnd)
+void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double err)
 {
-    double err = std::abs(depth - depthGnd) / depthGnd;
     err = min(err, 1.0);
     depth = min(depth, maxDepth_);
     cv::circle(curMask_, pt1, 3, cv::Scalar(255, 0, 0), -1);
diff --git a/src/module/stereo_module.h b/src/module/stereo_module.h
index 4aa6794..4e490ee 100644
--- a/src/module/stereo_module.h
+++ b/src/module/stereo_module.h
@@ -38,7 +38,7 @@ class StereoModule
     {
     public:
         void Init(cv::Size img_size);
-        void AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double depthGnd);
+        void AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double err);
         void Draw(cv::Mat &img, const cv::Mat &stereo_img);
 
     private:
diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc
index 01e1fc3..b5c5a4f 100644
--- a/src/module/tracking_module.cc
+++ b/src/module/tracking_module.cc
@@ -79,6 +79,7 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
             int tinx = min((int)(ti - ts_.begin()), (int)(ts_.size() - 1)) - 1;
             regionCount_[{rinx, tinx}]++;
 
+            const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID());
             Vector2d pixelGnd;
             if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd))
             {
@@ -86,7 +87,6 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
                 pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y;
                 double error = (pixel - pixelGnd).norm();
 
-                const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID());
                 visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i);
 
                 double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5;
@@ -95,6 +95,10 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
                 stats_.radialErrors.emplace_back(vector<double>{rg, error});
                 stats_.frameErrors.emplace_back(vector<double>{(double)landmark.GetNumObservations() - 1, (double)i, rg, error});
             }
+            else if (!frames_.back()->HasPose())
+            {
+                visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i);
+            }
             stats_.trackLengths[i]++;
             numGood++;
         }
@@ -181,6 +185,13 @@ void TrackingModule::Visualization::AddTrack(cv::Point2f gnd, cv::Point2f prev,
     cv::circle(curMask_, gnd, 3, colors_[index % colors_.size()], -1);
 }
 
+void TrackingModule::Visualization::AddTrack(cv::Point2f prev, cv::Point2f cur, int index)
+{
+    cv::Scalar color(255, 0, 0);
+    cv::line(visMask_, prev, cur, color, 1);
+    cv::circle(curMask_, cur, 1, color, -1);
+}
+
 void TrackingModule::Visualization::Draw(cv::Mat &img)
 {
     if (img.channels() == 1)
diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h
index 8fe464d..41eef62 100644
--- a/src/module/tracking_module.h
+++ b/src/module/tracking_module.h
@@ -46,6 +46,7 @@ class TrackingModule
     public:
         void Init(cv::Size img_size, int num_colors);
         void AddTrack(cv::Point2f gnd, cv::Point2f prev, cv::Point2f cur, double error, int index);
+        void AddTrack(cv::Point2f prev, cv::Point2f cur, int index);
         void Draw(cv::Mat &img);
 
     private:
diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc
index 355d85d..e5968bf 100644
--- a/src/odometry/pnp.cc
+++ b/src/odometry/pnp.cc
@@ -50,6 +50,7 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
             }
             else
             {
+                i++;
                 continue;
             }
             const data::Feature *feat = landmark.GetObservationByFrameID(frame.GetID());
diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc
index 5fc702b..d88e7e6 100644
--- a/src/optimization/bundle_adjuster.cc
+++ b/src/optimization/bundle_adjuster.cc
@@ -35,7 +35,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
         bool hasEstCameraPoses = false;
         for (const data::Feature &feature : landmark.GetObservations())
         {
-            if (feature.GetFrame().HasEstimatedPose())
+            if (feature.GetFrame().HasEstimatedPose() && feature.GetFrame().IsEstimatedByLandmark(landmark.GetID()))
             {
                 hasEstCameraPoses = true;
             }
@@ -94,7 +94,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
                 }
                 if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
                 {
-                    const Matrix<double, 3, 4> &pose = feature.GetFrame().GetInversePose();
+                    const Matrix<double, 3, 4> &pose = feature.GetFrame().GetEstimatedInversePose();
                     Quaterniond quat(pose.block<3, 3>(0, 0));
                     quat.normalize();
                     const Vector3d &t = pose.block<3, 1>(0, 3);
diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc
index ec8820f..f71d7ee 100644
--- a/src/ros/eval_base.cc
+++ b/src/ros/eval_base.cc
@@ -8,6 +8,8 @@
 #include "util/tf_util.h"
 #include "util/hdf_file.h"
 
+#define USE_GROUND_TRUTH
+
 using namespace Eigen;
 
 namespace omni_slam
@@ -103,8 +105,19 @@ void EvalBase<Stereo>::FrameCallback(const sensor_msgs::ImageConstPtr &image, co
     cv::Mat depthFloatImg;
     cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535);
 
+#ifdef USE_GROUND_TRUTH
     ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_)));
-
+#else
+    if (first_)
+    {
+        ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, posemat, pose->header.stamp.toSec(), *cameraModel_)));
+        first_ = false;
+    }
+    else
+    {
+        ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, pose->header.stamp.toSec(), *cameraModel_)));
+    }
+#endif
     Visualize(cvImage);
 }
 
@@ -136,7 +149,19 @@ void EvalBase<Stereo>::FrameCallback(const sensor_msgs::ImageConstPtr &image, co
     cv::Mat depthFloatImg;
     cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535);
 
+#ifdef USE_GROUND_TRUTH
     ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
+#else
+    if (first_)
+    {
+        ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
+        first_ = false;
+    }
+    else
+    {
+        ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
+    }
+#endif
 
     Visualize(cvImage, cvStereoImage);
 }
diff --git a/src/ros/eval_base.h b/src/ros/eval_base.h
index 0888470..c007515 100644
--- a/src/ros/eval_base.h
+++ b/src/ros/eval_base.h
@@ -66,6 +66,8 @@ class EvalBase
     std::string poseTopic_;
     std::map<std::string, double> cameraParams_;
     Matrix<double, 3, 4> stereoPose_;
+
+    bool first_{true};
 };
 
 }
diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc
index da33de8..ce80d71 100644
--- a/src/ros/odometry_eval.cc
+++ b/src/ros/odometry_eval.cc
@@ -62,6 +62,8 @@ void OdometryEval<Stereo>::ProcessFrame(unique_ptr<data::Frame> &&frame)
     this->trackingModule_->Update(frame);
     odometryModule_->Update(this->trackingModule_->GetLandmarks(), *this->trackingModule_->GetFrames().back());
     this->trackingModule_->Redetect();
+
+    this->visualized_ = false;
 }
 
 template <bool Stereo>
@@ -90,9 +92,9 @@ void OdometryEval<Stereo>::PublishOdometry()
 {
     geometry_msgs::PoseStamped poseMsg;
     poseMsg.header.frame_id = "map";
-    if (this->trackingModule_->GetFrames().back()->HasEstimatedPose())
+    if (this->trackingModule_->GetFrames().back()->HasEstimatedPose() || this->trackingModule_->GetFrames().size() == 1)
     {
-        const Matrix<double, 3, 4> &pose = this->trackingModule_->GetFrames().back()->GetEstimatedPose();
+        const Matrix<double, 3, 4> &pose = (this->trackingModule_->GetFrames().size() == 1 && !this->trackingModule_->GetFrames().back()->HasEstimatedPose()) ? this->trackingModule_->GetFrames().back()->GetPose() : this->trackingModule_->GetFrames().back()->GetEstimatedPose();
         poseMsg.pose.position.x = pose(0, 3);
         poseMsg.pose.position.y = pose(1, 3);
         poseMsg.pose.position.z = pose(2, 3);
@@ -122,14 +124,15 @@ void OdometryEval<Stereo>::PublishOdometry()
     nav_msgs::Path pathGnd;
     pathGnd.header.stamp = ::ros::Time::now();
     pathGnd.header.frame_id = "map";
+    bool first = true;
     for (const std::unique_ptr<data::Frame> &frame : this->trackingModule_->GetFrames())
     {
-        if (frame->HasEstimatedPose())
+        if (frame->HasEstimatedPose() || first)
         {
             geometry_msgs::PoseStamped poseMsg;
             poseMsg.header.stamp = ::ros::Time(frame->GetTime());
             poseMsg.header.frame_id = "map";
-            const Matrix<double, 3, 4> &pose = frame->GetEstimatedPose();
+            const Matrix<double, 3, 4> &pose = (first && !frame->HasEstimatedPose()) ? frame->GetPose() : frame->GetEstimatedPose();
             poseMsg.pose.position.x = pose(0, 3);
             poseMsg.pose.position.y = pose(1, 3);
             poseMsg.pose.position.z = pose(2, 3);
@@ -156,6 +159,7 @@ void OdometryEval<Stereo>::PublishOdometry()
             poseMsg.pose.orientation.w = quat.normalized().w();
             pathGnd.poses.push_back(poseMsg);
         }
+        first = false;
     }
     pathPublisher_.publish(path);
     pathGndPublisher_.publish(pathGnd);
diff --git a/src/ros/reconstruction_eval.cc b/src/ros/reconstruction_eval.cc
index 7bda19b..229f4b6 100644
--- a/src/ros/reconstruction_eval.cc
+++ b/src/ros/reconstruction_eval.cc
@@ -53,6 +53,8 @@ void ReconstructionEval<Stereo>::ProcessFrame(unique_ptr<data::Frame> &&frame)
     this->trackingModule_->Update(frame);
     reconstructionModule_->Update(this->trackingModule_->GetLandmarks());
     this->trackingModule_->Redetect();
+
+    this->visualized_ = false;
 }
 
 template <bool Stereo>
diff --git a/src/stereo/stereo_matcher.cc b/src/stereo/stereo_matcher.cc
index ce6f37e..39c73df 100644
--- a/src/stereo/stereo_matcher.cc
+++ b/src/stereo/stereo_matcher.cc
@@ -18,6 +18,19 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
     {
         return 0;
     }
+    Matrix<double, 3, 4> framePose;
+    if (frame.HasEstimatedPose())
+    {
+        framePose = frame.GetEstimatedPose();
+    }
+    else if (frame.HasPose())
+    {
+        framePose = frame.GetPose();
+    }
+    else
+    {
+        return 0;
+    }
     std::vector<cv::KeyPoint> pointsToMatch;
     std::vector<Vector3d> bearings1;
     std::vector<int> origInx;
@@ -45,9 +58,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
     std::vector<int> matchedIndices;
     FindMatches(frame.GetImage(), frame.GetStereoImage(), pointsToMatch, matchedPoints, matchedIndices);
 
-    Matrix<double, 3, 4> I;
-    I.block<3, 3>(0, 0) = Matrix3d::Identity();
-    I.block<3, 1>(0, 3) = Vector3d::Zero();
+    Matrix<double, 3, 4> I = util::TFUtil::IdentityPoseMatrix<double>();
     Matrix3d E = util::TFUtil::GetEssentialMatrixFromPoses(I, frame.GetStereoPose());
     int good = 0;
     #pragma omp parallel for reduction(+:good)
@@ -66,8 +77,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
         double epiErr2 = std::abs(bearing2.transpose() * epiplane2);
         if (epiErr1 < epipolarThresh_ && epiErr2 < epipolarThresh_)
         {
-            Matrix<double, 3, 4> framePose = frame.HasEstimatedPose() ? frame.GetEstimatedPose() : frame.GetPose();
-            landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose())));
+            landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose())), std::vector<int>({frame.GetID()}));
             landmarks[origInx[inx]].AddStereoObservation(feat);
             good++;
         }
diff --git a/src/util/tf_util.h b/src/util/tf_util.h
index a42384c..6a45c3e 100644
--- a/src/util/tf_util.h
+++ b/src/util/tf_util.h
@@ -88,6 +88,15 @@ inline Matrix<T, 3, 3> GetEssentialMatrixFromPoses(Matrix<T, 3, 4> pose1, Matrix
     return tx * GetRotationFromPoseMatrix(rel);
 }
 
+template <typename T>
+inline Matrix<T, 3, 4> IdentityPoseMatrix()
+{
+    Matrix<T, 3, 4> I;
+    I.template block<3, 3>(0, 0) = Matrix3d::Identity();
+    I.template block<3, 1>(0, 3) = Vector3d::Zero();
+    return I;
+}
+
 }
 }
 }