Use homogeneous pixels for Outlier rejection

Makes it more modular
Add getters for eigen points on WKeyPoints
Add computeRay to sensorCamera
Edit filterWithEssential

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;}
 

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_;}

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

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