diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h
index 2cc479082546c8955e807ad124d11102a17a2733..fa938812d54188675d06eeec09cef3d05850f4de 100644
--- a/include/vision/capture/capture_image.h
+++ b/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;}
diff --git a/include/vision/sensor/sensor_camera.h b/include/vision/sensor/sensor_camera.h
index adf8527596f4d3915083073c839d94d6c3128d25..107ec7d06a0840d96d1baf86ec80bbd7adc2bb53 100644
--- a/include/vision/sensor/sensor_camera.h
+++ b/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;}
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 2192ab6af4cdef0962ec61df6350309a54eba44b..9d78da8163e258d588231e9436825f6873a9063a 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/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_,
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 888a4cf5dd73d8d71beb0a3ccf11ba8562fa3ff0..8c57c542eae854c48476ad4caf1023fc7bac1ec0 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/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
diff --git a/test/gtest_processor_visual_odometry.cpp b/test/gtest_processor_visual_odometry.cpp
index 4a584eb7a8848c649b7ca425c450bc35861b9b84..76c2713c056fe96d5dc465517d9ddf39f57e384c 100644
--- a/test/gtest_processor_visual_odometry.cpp
+++ b/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