diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index d58369e4a99cf87c6c9966aea46d5ebeafa513d8..5368e965f2433b14498d7fcff23d70ed6fdddb65 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -35,23 +35,37 @@
// wolf includes
#include <core/math/rotations.h>
+#include <core/math/SE3.h>
+#include <core/state_block/state_composite.h>
#include <core/processor/processor_tracker.h>
#include <core/processor/track_matrix.h>
+#include <core/processor/motion_provider.h>
// Opencv includes
#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
+#include <opencv2/core/eigen.hpp>
namespace wolf{
+/** \brief Buffer of VectorComposite
+ *
+ * Object and functions to manage a buffer of VectorComposite objects.
+ * Used to hold a memory of origin->last relative poses.
+ */
+class BufferVectorCompositePtr : public Buffer<std::shared_ptr<VectorComposite>> { };
+
+
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorVisualOdometry);
-struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
+struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker, public ParamsMotionProvider
{
struct RansacParams
{
@@ -107,7 +121,8 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
ParamsProcessorVisualOdometry() = default;
ParamsProcessorVisualOdometry(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorTracker(_unique_name, _server)
+ ParamsProcessorTracker(_unique_name, _server),
+ ParamsMotionProvider(_unique_name, _server)
{
std_pix = _server.getParam<double>(prefix + _unique_name + "/std_pix");
@@ -171,12 +186,18 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
WOLF_PTR_TYPEDEFS(ProcessorVisualOdometry);
-class ProcessorVisualOdometry : public ProcessorTracker
+class ProcessorVisualOdometry : public ProcessorTracker, public MotionProvider
{
public:
ProcessorVisualOdometry(ParamsProcessorVisualOdometryPtr _params_visual_odometry);
virtual ~ProcessorVisualOdometry() override {};
+ // MotionProvider class pure virtual methods
+ VectorComposite getState(const StateStructure& _structure = "") const override;
+ TimeStamp getTimeStamp( ) const override;
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override;
+ VectorComposite getRelativePoseOriginLast() const;
+
WOLF_PROCESSOR_CREATE(ProcessorVisualOdometry, ParamsProcessorVisualOdometry);
protected:
@@ -204,6 +225,9 @@ class ProcessorVisualOdometry : public ProcessorTracker
// bookeeping
TracksMap tracks_map_li_matched_;
+ // buffer of origin->last camera relative poses mapping origin to last
+ BufferVectorCompositePtr buffer_pose_cam_ol_;
+
public:
@@ -300,7 +324,11 @@ class ProcessorVisualOdometry : public ProcessorTracker
/** \brief Remove outliers from the tracks map with a RANSAC 5-points algorithm implemented on openCV
*/
- bool filterWithEssential(const KeyPointsMap mwkps_prev, const KeyPointsMap mwkps_curr, TracksMap &tracks_prev_curr, cv::Mat &E);
+ bool filterWithEssential(const KeyPointsMap mwkps_prev,
+ const KeyPointsMap mwkps_curr,
+ TracksMap &tracks_prev_curr,
+ cv::Mat &E,
+ VectorComposite &_pose_prev_curr);
/** \brief Tool to merge tracks
*/
@@ -310,6 +338,14 @@ class ProcessorVisualOdometry : public ProcessorTracker
const TrackMatrix& getTrackMatrix() const {return track_matrix_;}
+ VectorComposite getStateFromRelativeOriginLast(VectorComposite co_pose_cl) const;
+
+ static VectorComposite pose_from_essential_matrix(const cv::Mat& _E,
+ const std::vector<cv::Point2d>& p2d_prev,
+ const std::vector<cv::Point2d>& p2d_curr,
+ const cv::Mat& _K,
+ cv::Mat& cvMask);
+
private:
void retainBest(std::vector<cv::KeyPoint> &_keypoints, int n);
@@ -330,7 +366,6 @@ class ProcessorVisualOdometry : public ProcessorTracker
void detect_keypoints_in_empty_grid_cells(cv::Mat img_last, const TracksMap& tracks_last_incoming_filtered, const KeyPointsMap& mwkps_last,
std::vector<cv::KeyPoint>& kps_last_new, KeyPointsMap& mwkps_last_filtered);
-
};
} //namespace wolf
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 44688712b2cfdf5644ecf07471cdff6ac27f8250..6af48425384750639bd07f5e71926926510c7a72 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -24,17 +24,16 @@
//standard
#include "vision/processor/processor_visual_odometry.h"
-#include <opencv2/imgproc.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/core/eigen.hpp>
-
#include <chrono>
#include <ctime>
namespace wolf{
+using namespace SE3;
+
ProcessorVisualOdometry::ProcessorVisualOdometry(ParamsProcessorVisualOdometryPtr _params_vo) :
ProcessorTracker("ProcessorVisualOdometry", "PO", 3, _params_vo),
+ MotionProvider("PO", _params_vo),
params_visual_odometry_(_params_vo)
{
// Preprocessor stuff
@@ -146,7 +145,16 @@ void ProcessorVisualOdometry::preProcess()
// Outliers rejection with essential matrix
cv::Mat E;
- filterWithEssential(mwkps_origin, mwkps_incoming, tracks_origin_incoming, E);
+ VectorComposite pose_ol("PO", {3,4});
+ bool success = filterWithEssential(mwkps_origin, mwkps_incoming, tracks_origin_incoming, E, pose_ol);
+ if (success){
+ // store result of recoverPose if RANSAC was handled well
+ buffer_pose_cam_ol_.emplace(origin_ptr_->getTimeStamp(), std::make_shared<VectorComposite>(pose_ol));
+ }
+ else{
+ WOLF_WARN("!!!!!!Essential matrix RANSAC failed!!!!!!!!");
+ // return; // What should we do in this case?
+ }
// Edit tracks prev with only inliers wrt origin
// and remove also from mwkps_incoming all the keypoints that have not been tracked
@@ -193,7 +201,8 @@ void ProcessorVisualOdometry::preProcess()
// Outliers rejection with essential matrix
// tracks that are not geometrically consistent are removed from tracks_last_incoming_new
- filterWithEssential(mwkps_last_filtered, mwkps_incoming_filtered, tracks_last_incoming_filtered, E);
+ VectorComposite not_used;
+ filterWithEssential(mwkps_last_filtered, mwkps_incoming_filtered, tracks_last_incoming_filtered, E, not_used);
WOLF_DEBUG("New total : ", n_tracks_origin, " + ", mwkps_incoming_new.size(), " = ", tracks_last_incoming_filtered.size(), " tracks");
@@ -492,7 +501,6 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkNaive(FeaturePointImageP
}
-
Eigen::Isometry3d ProcessorVisualOdometry::getTcw(TimeStamp _ts) const
{
// get robot position and orientation at capture's timestamp
@@ -642,7 +650,11 @@ TracksMap ProcessorVisualOdometry::kltTrack(const cv::Mat _img_prev, const cv::M
return tracks_prev_curr;
}
-bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev, const KeyPointsMap _mwkps_curr, TracksMap &_tracks_prev_curr, cv::Mat &_E)
+bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev,
+ const KeyPointsMap _mwkps_curr,
+ TracksMap &_tracks_prev_curr,
+ cv::Mat &_E,
+ VectorComposite &_pose_prev_curr)
{
// We need at least five tracks
// TODO: this case is NOT handled by the rest of the algorithm currently
@@ -696,6 +708,12 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
prms.thresh,
cvMask);
+ // recover the pose relative pose if asked
+ if (_pose_prev_curr.includesStructure("PO")){
+ // modifies the mask of inliers -> maybe to change?
+ _pose_prev_curr = pose_from_essential_matrix(_E, p2d_prev, p2d_curr, Kcv_, cvMask);
+ }
+
// Let's remove outliers from tracksMap
for (size_t k=0; k<all_indices.size(); k++){
if (cvMask.at<bool>(k) == 0){
@@ -706,6 +724,29 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
return true;
}
+
+VectorComposite ProcessorVisualOdometry::pose_from_essential_matrix(const cv::Mat& _E,
+ const std::vector<cv::Point2d>& _p2d_prev,
+ const std::vector<cv::Point2d>& _p2d_curr,
+ const cv::Mat& _K,
+ cv::Mat& _cvMask)
+{
+ cv::Mat R_out, t_out;
+ cv::recoverPose(_E, _p2d_prev, _p2d_curr, _K, R_out, t_out, _cvMask);
+
+ // translation into eigen objects
+ Eigen::Matrix3d R_eig;
+ Eigen::Vector3d t_eig;
+ cv::cv2eigen(R_out, R_eig);
+ cv::cv2eigen(t_out, t_eig);
+
+ VectorComposite pose_prev_curr("PO", {3,4});
+ pose_prev_curr['P'] = t_eig;
+ pose_prev_curr['O'] = Quaterniond(R_eig).coeffs();
+ return pose_prev_curr;
+}
+
+
void ProcessorVisualOdometry::retainBest(std::vector<cv::KeyPoint> &_keypoints, int n)
{
if (_keypoints.size() > n) {
@@ -853,6 +894,95 @@ void ProcessorVisualOdometry::detect_keypoints_in_empty_grid_cells(cv::Mat img_l
}
+
+
+
+
+
+/////////////////////////
+// MotionProvider methods
+/////////////////////////
+
+VectorComposite ProcessorVisualOdometry::getState( const StateStructure& _structure ) const
+{
+ return getState(getTimeStamp(), _structure);
+}
+
+TimeStamp ProcessorVisualOdometry::getTimeStamp() const
+{
+ if ( last_ptr_ == nullptr )
+ return TimeStamp::Invalid();
+ else
+ return last_ptr_->getTimeStamp();
+}
+
+VectorComposite ProcessorVisualOdometry::getState(const TimeStamp& _ts, const StateStructure& _structure) const
+{
+ // valid last...
+ if (last_ptr_ != origin_ptr_)
+ {
+
+ std::shared_ptr<VectorComposite> pose_cam_ol_ptr = buffer_pose_cam_ol_.selectFirstBefore(_ts, getTimeTolerance());
+
+ if( !pose_cam_ol_ptr )
+ {
+ WOLF_WARN(getName(), ": Requested state with time stamp out of tolerance. Returning empty VectorComposite");
+ return VectorComposite();
+ }
+ else
+ {
+ return *pose_cam_ol_ptr;
+ }
+ }
+ // return state at origin if possible
+ else
+ {
+ if (origin_ptr_ == nullptr || fabs(_ts - origin_ptr_->getTimeStamp()) > params_->time_tolerance)
+ {
+ WOLF_WARN(getName(), ": Requested state with time stamp out of tolerance. Returning empty VectorComposite");
+ return VectorComposite();
+ }
+ else
+ return origin_ptr_->getFrame()->getState("PO");
+ }
+}
+
+
+
+
+
+VectorComposite ProcessorVisualOdometry::getStateFromRelativeOriginLast(VectorComposite co_pose_cl) const
+{
+ if (origin_ptr_ == nullptr or
+ origin_ptr_->isRemoving() or
+ last_ptr_ == nullptr or
+ last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state
+ // Further checking here for origin_ptr is redundant: if last=null, then origin=null too.
+ {
+ WOLF_WARN("Processor has no state. Returning an empty VectorComposite");
+ return VectorComposite(); // return empty state
+ }
+
+ VectorComposite r_pose_c = getSensor()->getState();
+ VectorComposite c_pose_r = inverse(r_pose_c);
+
+ // composte poses to obtain the relative robot transformation
+ VectorComposite ro_pose_rl = SE3::compose(r_pose_c, SE3::compose(co_pose_cl, c_pose_r));
+
+ // Pose at origin
+ VectorComposite w_pose_ro = getOrigin()->getFrame()->getState();
+
+ // Pose at last using composition
+ VectorComposite w_pose_rl = SE3::compose(w_pose_ro, ro_pose_rl);
+
+ WOLF_INFO("I WAS CALLED!!!")
+
+ return w_pose_rl;
+}
+
+
+
+
} //namespace wolf
// Register in the FactoryProcessor
diff --git a/test/gtest_processor_visual_odometry.cpp b/test/gtest_processor_visual_odometry.cpp
index e56f8dc3fd4f293da7e96d4ad3562d4b58aa9eb2..9e6d167cc90d4ef7e3a586c859765a60fe22f203 100644
--- a/test/gtest_processor_visual_odometry.cpp
+++ b/test/gtest_processor_visual_odometry.cpp
@@ -429,19 +429,21 @@ TEST_F(ProcessorVOMultiView_class, filterWithEssential)
cv::Mat E;
// Let's try FilterWithEssential, all points here are inliers
- processor->filterWithEssential(mwkps_c1, mwkps_c2, tracks_c1_c2, E);
+ VectorComposite toto; // empty VectorComposite -> does not recoverPose
+ bool success = processor->filterWithEssential(mwkps_c1, mwkps_c2, tracks_c1_c2, E, toto);
+ ASSERT_TRUE(success);
ASSERT_EQ(tracks_c1_c2.size(), mwkps_c2.size());
////////////////////////////////////////////////////////////////////
// We had here an outlier: a keyPoint that doesn't move
- mwkps_c1.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2f(120,140), 1))));
- mwkps_c2.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2f(120,140), 1))));
+ mwkps_c1.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2d(120,140), 1))));
+ mwkps_c2.insert(std::pair<size_t, WKeyPoint>(8, WKeyPoint(cv::KeyPoint(cv::Point2d(120,140), 1))));
tracks_c1_c2.insert(std::pair<size_t, size_t>(8,8));
// point at 8 must be discarded
- processor->filterWithEssential(mwkps_c1, mwkps_c2, tracks_c1_c2, E);
+ success = processor->filterWithEssential(mwkps_c1, mwkps_c2, tracks_c1_c2, E, toto);
+ ASSERT_TRUE(success);
ASSERT_EQ(tracks_c1_c2.count(8), 0);
-
}
@@ -453,11 +455,12 @@ TEST_F(ProcessorVOMultiView_class, recoverPoseOpenCV)
cv::Mat E;
// Compute essential matrix, all points here are inliers
- processor->filterWithEssential(mwkps_c1, mwkps_c2, tracks_c1_c2, E);
+ VectorComposite titi;
+ processor->filterWithEssential(mwkps_c1, mwkps_c2, tracks_c1_c2, E, titi);
////////////////////////////////////////////////////////////////////
// can we retrieve the right orientation from the essential matrix?
- std::vector<cv::Point2f> pts_c1, pts_c2;
+ std::vector<cv::Point2d> pts_c1, pts_c2;
for (int i=0; i < mwkps_c1.size(); i++){
pts_c1.push_back(mwkps_c1.at(i).getCvKeyPoint().pt);
pts_c2.push_back(mwkps_c2.at(i).getCvKeyPoint().pt);
@@ -472,6 +475,11 @@ TEST_F(ProcessorVOMultiView_class, recoverPoseOpenCV)
cv::cv2eigen(R_out, R_out_eig);
ASSERT_MATRIX_APPROX(R_21, R_out_eig, 1e-4);
+
+ // Same with helper function
+ VectorComposite pose_21_out = ProcessorVisualOdometry::pose_from_essential_matrix(E, pts_c1, pts_c2, Kcv, mask);
+
+
//////////////////////////////////////////////////////
// Can we also use it to detect outliers using cheirality check?
// Does not seem so...
@@ -482,26 +490,28 @@ TEST_F(ProcessorVOMultiView_class, recoverPoseOpenCV)
// Can simply mean an absence of movement, hard to tune threshold...
// add outliers
- pts_c1.push_back(cv::Point2f(165.0, 190.0));
- pts_c2.push_back(cv::Point2f(200.0, 190.0));
+ pts_c1.push_back(cv::Point2d(165.0, 190.0));
+ pts_c2.push_back(cv::Point2d(200.0, 190.0));
- pts_c1.push_back(cv::Point2f(100.0, 250.0));
- pts_c2.push_back(cv::Point2f(100.0, 250.0));
+ pts_c1.push_back(cv::Point2d(100.0, 250.0));
+ pts_c2.push_back(cv::Point2d(100.0, 250.0));
- pts_c1.push_back(cv::Point2f(400.0, 70.0));
- pts_c2.push_back(cv::Point2f(400.0, 70.0));
+ pts_c1.push_back(cv::Point2d(400.0, 70.0));
+ pts_c2.push_back(cv::Point2d(400.0, 70.0));
- pts_c1.push_back(cv::Point2f(300.0, 300.0));
- pts_c2.push_back(cv::Point2f(100.0, 300.0));
+ pts_c1.push_back(cv::Point2d(300.0, 300.0));
+ pts_c2.push_back(cv::Point2d(100.0, 300.0));
+ mask = 255*cv::Mat::ones(12, 1, CV_64F);
cv::Mat triangulated_pts;
double distance_threshold = 80.0;
// cv::recoverPose(E, pts_c1, pts_c2, Kcv, R_out, t_out, mask);
- cv::recoverPose(E, pts_c1, pts_c2, Kcv, R_out, t_out, distance_threshold, mask, triangulated_pts);
+ cv::Mat new_mask;
+ cv::recoverPose(E, pts_c1, pts_c2, Kcv, R_out, t_out, distance_threshold, new_mask, triangulated_pts);
// triangulated_pts.size()
- std::cout << triangulated_pts.size() << std::endl;
+ // std::cout << triangulated_pts.size() << std::endl;
// std::cout << "mask" << std::endl;
@@ -511,9 +521,6 @@ TEST_F(ProcessorVOMultiView_class, recoverPoseOpenCV)
// std::cout << R_out << std::endl;
// std::cout << "t_out" << std::endl;
// std::cout << t_out << std::endl;
-
-
-
}