From c5c3d3e6de11f622aa9d24a5884a11a88833163f Mon Sep 17 00:00:00 2001
From: cesardebeunne <debeunnecesar@gmail.com>
Date: Fri, 22 Apr 2022 11:24:36 +0200
Subject: [PATCH] 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 | 3 +++
include/vision/sensor/sensor_camera.h | 1 +
src/processor/processor_visual_odometry.cpp | 12 +++++++++---
src/sensor/sensor_camera.cpp | 7 +++++++
4 files changed, 20 insertions(+), 3 deletions(-)
diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h
index 66415c5d7..2cc479082 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 91b23e254..adf852759 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 a0ea2f2b3..2192ab6af 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 8c57c542e..888a4cf5d 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
--
GitLab