diff --git a/src/feature/lk_tracker.cc b/src/feature/lk_tracker.cc
index 9ca6f17..4fc873c 100644
--- a/src/feature/lk_tracker.cc
+++ b/src/feature/lk_tracker.cc
@@ -127,6 +127,13 @@ int LKTracker::DoTrack(std::vector<data::Landmark> &landmarks, data::Frame &cur_
                 }
             }
         }
+        Vector2d pixelCur;
+        pixelCur << results[i].x, results[i].y;
+        Vector3d tmp;
+        if (!cur_frame.GetCameraModel().UnprojectToBearing(pixelCur, tmp))
+        {
+            continue;
+        }
         if (status[i] == 1 && err[i] <= errThresh_)
         {
             cv::KeyPoint kpt(results[i], origKpt[i].size);
diff --git a/src/module/stereo_module.cc b/src/module/stereo_module.cc
index d950d26..0c6492e 100644
--- a/src/module/stereo_module.cc
+++ b/src/module/stereo_module.cc
@@ -72,7 +72,10 @@ void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, dou
     cv::circle(curMask_, pt1, 3, cv::Scalar(255, 0, 0), -1);
     cv::circle(curMask_, pt2 + cv::Point2f(curMask_.cols / 2., 0), 3, cv::Scalar(255, 0, 0), -1);
     cv::line(curMask_, pt1, pt2 + cv::Point2f(curMask_.cols / 2., 0), cv::Scalar(255, 0, 0), 1);
-    cv::circle(curDepth_, pt1, 2, cv::Scalar((depth / maxDepth_ - err) * 255, (depth / maxDepth_ - err) * 255, depth / maxDepth_ * 255));
+    double depthColor = 1 - depth / maxDepth_;
+    depthColor *= 0.9 * depthColor;
+    depthColor += 0.1;
+    cv::circle(curDepth_, pt1, 2, cv::Scalar((depthColor - err) * 255, (depthColor - err) * 255, depthColor * 255));
 }
 
 void StereoModule::Visualization::Draw(cv::Mat &img, const cv::Mat &stereo_img)