Switch to double in preprocess()

Use pinhole_tools.h instead of sensorCamera

include/vision/capture/capture_image.h

@@ -60,8 +60,8 @@ 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;}
+        Eigen::Vector2d getEigenKeyPoint() const {return Eigen::Vector2d(cv_kp_.pt.x, cv_kp_.pt.y);}
+        cv::Point2d 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

@@ -115,13 +115,12 @@ class SensorCamera : public SensorBase
         Eigen::VectorXd getDistortionVector()   { return distortion_; }
         Eigen::VectorXd getCorrectionVector()   { return correction_; }
         Eigen::Matrix3d getIntrinsicMatrix()    { return K_; }
+        Eigen::Vector4d getPinholeModel()       { return pinhole_model_raw_; }
 
         bool isUsingRawImages() { return using_raw_; }
         bool useRawImages();
         bool useRectifiedImages();
 
-        Eigen::Vector3f computeRay(Eigen::Vector2f _pix);
-
         int getImgWidth(){return img_width_;}
         int getImgHeight(){return img_height_;}
         void setImgWidth(int _w){img_width_ = _w;}

src/processor/processor_visual_odometry.cpp

@@ -646,27 +646,27 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
 {
     ParamsProcessorVisualOdometry::RansacParams prms = params_visual_odometry_->ransac_params_;
 
-    // We need to build lists of pt2f for openCV function
-    std::vector<cv::Point2f> p2f_prev, p2f_curr;
+    // We need to build lists of pt2d for openCV function
+    std::vector<cv::Point2d> p2d_prev, p2d_curr;
     std::vector<size_t> all_indices;
     for (auto & track : _tracks_prev_curr){
         all_indices.push_back(track.first);
-        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()));
+        Eigen::Vector2d ray_prev = pinhole::depixellizePoint(sen_cam_->getPinholeModel(), _mwkps_prev.at(track.first).getEigenKeyPoint());
+        Eigen::Vector2d ray_curr = pinhole::depixellizePoint(sen_cam_->getPinholeModel(), _mwkps_curr.at(track.second).getEigenKeyPoint());
+        p2d_prev.push_back(cv::Point2d(ray_prev.x(), ray_prev.y()));
+        p2d_curr.push_back(cv::Point2d(ray_curr.x(), ray_curr.y()));
     }
 
     // We need at least five tracks
-    if (p2f_prev.size() < 5) return false;
+    if (p2d_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) +
+    double focal = (sen_cam_->getIntrinsicMatrix()(0,0) +
                    sen_cam_->getIntrinsicMatrix()(1,1)) / 2;
 
-    _E = cv::findEssentialMat(p2f_prev, 
-                              p2f_curr, 
+    _E = cv::findEssentialMat(p2d_prev, 
+                              p2d_curr, 
                               Kcv_, 
                               cv::RANSAC,
                               prms.ransac_prob_,

src/sensor/sensor_camera.cpp

@@ -71,13 +71,6 @@ 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

test/gtest_processor_visual_odometry.cpp

@@ -404,8 +404,8 @@ TEST(ProcessorVisualOdometry, filterWithEssential)
     ASSERT_EQ(tracks_prev_curr.size(), mwkps_curr.size());
 
     // We had here an outlier: a keyPoint that doesn't move
-    mwkps_prev.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2f(120,140), 1))));
-    mwkps_curr.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2f(120,140), 1))));
+    mwkps_prev.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2d(120,140), 1))));
+    mwkps_curr.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2d(120,140), 1))));
     tracks_prev_curr.insert(std::pair<size_t, size_t>(8,8)); 
 
     // point at 8 must be discarded