diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h
index 66415c5d7c0a16702c4c0630557ea461b0999cd3..2cc479082546c8955e807ad124d11102a17a2733 100644
--- a/include/vision/capture/capture_image.h
+++ b/include/vision/capture/capture_image.h
@@ -60,6 +60,9 @@ class WKeyPoint
         cv::KeyPoint getCvKeyPoint() const {return cv_kp_;}
         void setCvKeyPoint(cv::KeyPoint _cv_kp) {cv_kp_ = _cv_kp;}
 
+        Eigen::Vector2f getEigenKeyPoint() const {return Eigen::Vector2f(cv_kp_.pt.x, cv_kp_.pt.y);}
+        cv::Point2f getCvPoint() const {return cv_kp_.pt;}
+
         cv::Mat getDescriptor() const {return desc_;}
         void setDescriptor(cv::Mat _desc) {desc_ = _desc;}
 
diff --git a/include/vision/sensor/sensor_camera.h b/include/vision/sensor/sensor_camera.h
index 91b23e254d9f86d7f4c621a5a4a49c26d1cf287b..adf8527596f4d3915083073c839d94d6c3128d25 100644
--- a/include/vision/sensor/sensor_camera.h
+++ b/include/vision/sensor/sensor_camera.h
@@ -120,6 +120,7 @@ class SensorCamera : public SensorBase
         bool useRawImages();
         bool useRectifiedImages();
 
+        Eigen::Vector3f computeRay(Eigen::Vector2f _pix);
 
         int getImgWidth(){return img_width_;}
         int getImgHeight(){return img_height_;}
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index a0ea2f2b38a0df188f2d24201b14319d571d9072..2192ab6af4cdef0962ec61df6350309a54eba44b 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -651,20 +651,26 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
     std::vector<size_t> all_indices;
     for (auto & track : _tracks_prev_curr){
         all_indices.push_back(track.first);
-        p2f_prev.push_back(_mwkps_prev.at(track.first).getCvKeyPoint().pt);
-        p2f_curr.push_back(_mwkps_curr.at(track.second).getCvKeyPoint().pt);
+        Eigen::Vector3f ray_prev = sen_cam_->computeRay(_mwkps_prev.at(track.first).getEigenKeyPoint());
+        Eigen::Vector3f ray_curr = sen_cam_->computeRay(_mwkps_curr.at(track.second).getEigenKeyPoint());
+        p2f_prev.push_back(cv::Point2f(ray_prev.x() / ray_prev.z(), ray_prev.y() / ray_prev.z()));
+        p2f_curr.push_back(cv::Point2f(ray_curr.x() / ray_curr.z(), ray_curr.y() / ray_curr.z()));
     }
 
     // We need at least five tracks
     if (p2f_prev.size() < 5) return false;
 
     cv::Mat cvMask;
+    cv::Mat K = cv::Mat::eye(3,3,CV_32F);
+    float focal = (sen_cam_->getIntrinsicMatrix()(0,0) +
+                   sen_cam_->getIntrinsicMatrix()(1,1)) / 2;
+
     _E = cv::findEssentialMat(p2f_prev, 
                               p2f_curr, 
                               Kcv_, 
                               cv::RANSAC,
                               prms.ransac_prob_,
-                              prms.ransac_thresh_,
+                              prms.ransac_thresh_ / focal,
                               cvMask);
     
     // Let's remove outliers from tracksMap
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 8c57c542eae854c48476ad4caf1023fc7bac1ec0..888a4cf5dd73d8d71beb0a3ccf11ba8562fa3ff0 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -71,6 +71,13 @@ Eigen::Matrix3d SensorCamera::setIntrinsicMatrix(Eigen::Vector4d _pinhole_model)
     K.row(2) << 0, 0, 1;
     return K;
 }
+
+Eigen::Vector3f SensorCamera::computeRay(Eigen::Vector2f _pix)
+{
+    Eigen::Vector3f ray;
+    ray = K_.cast<float>().inverse() * Eigen::Vector3f(_pix.x(), _pix.y(), 1);
+    return ray; 
+}
 } // namespace wolf
 
 // Register in the FactorySensor