diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index a7cde6c38636e7c6b24a4e5cba8b6b827f950f6e..9c6fd947338f3a2f9513fa800cf0c22a4d23254f 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -258,12 +258,15 @@ class ProcessorVisualOdometry : public ProcessorTracker
/**
* \brief Emplace a landmark corresponding to a track and initialize it with triangulation.
* \param _feature_ptr a pointer to the feature used to create the new landmark
+ * \param _track_kf track with only features associated to keyframes that maye be used for initialisation
* \return a pointer to the created landmark. If null, the triangulation failed due to low parallax
*
* Implementation: try to triangulate a new landmark based on previous frames estimates.
* Apply a numerical test to asses if parallax is enough.
*/
- LandmarkBasePtr emplaceLandmarkTriangulation(FeatureBasePtr _feature_ptr);
+ LandmarkBasePtr emplaceLandmarkTriangulation(FeatureBasePtr _feature_ptr, Track _track_kf);
+
+ Eigen::Isometry3d getTcw(TimeStamp _ts) const;
/** \brief Advance the incoming Capture to become the last.
*
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index cdefc35d32c1cb88726f28700afaff35db4f56ec..a8e398289a1bb892cac30fade2b081ea2106088c 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -25,6 +25,8 @@
#include "vision/processor/processor_visual_odometry.h"
#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
#include <chrono>
#include <ctime>
@@ -345,11 +347,19 @@ void ProcessorVisualOdometry::establishFactors()
else if(track_matrix_.trackSize(feat->trackId()) >= params_visual_odometry_->min_track_length_for_landmark)
{
WOLF_INFO(" NEW valid track \\o/")
- LandmarkBasePtr lmk = emplaceLandmarkNaive(feat_pi);
- lmk->setId(feat_pi->trackId());
+ Track track_kf = track_matrix_.trackAtKeyframes(feat->trackId());
+
+ LandmarkBasePtr lmk = emplaceLandmarkTriangulation(feat_pi, track_kf);
+ if (lmk == nullptr){
+ continue;
+ }
// Add factors from all KFs of this track to the new lmk
- Track track_kf = track_matrix_.trackAtKeyframes(feat->trackId());
+ WOLF_INFO("\n\nHEY")
+ WOLF_INFO(feat_pi->getFrame()->getStateVector().transpose())
+ WOLF_INFO(lmk->getStateVector().transpose())
+ WOLF_INFO(getSensor()->getStateVector().transpose())
+
for (auto feat_kf: track_kf)
{
LandmarkHpPtr lmk_hp = std::dynamic_pointer_cast<LandmarkHp>(lmk);
@@ -364,6 +374,76 @@ void ProcessorVisualOdometry::establishFactors()
return;
}
+
+LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkTriangulation(FeatureBasePtr _feat, Track _track_kf)
+{
+ // at least 2 KF needed to triangulate
+ if (_track_kf.size() < 2)
+ {
+ return nullptr;
+ }
+
+ // heuristic: use oldest feature/KF to triangulate with respect to current time
+ FeaturePointImagePtr feat_pi1 = std::static_pointer_cast<FeaturePointImage>(_track_kf.begin()->second);
+ // Feature at current time
+ FeaturePointImagePtr feat_pi2 = std::static_pointer_cast<FeaturePointImage>(_feat);
+
+ cv::Vec2d pix1, pix2;
+ // WOLF_INFO("TOTO")
+ cv::eigen2cv(feat_pi1->getMeasurement(), pix1);
+ cv::eigen2cv(feat_pi2->getMeasurement(), pix2);
+ // WOLF_INFO(pix1, pix2)
+
+ // create 3x4 projection matrices [K|0] * Tcw
+ Matrix4d Tcw1 = getTcw(feat_pi1->getCapture()->getTimeStamp()).matrix();
+ Matrix4d Tcw2 = getTcw(feat_pi2->getCapture()->getTimeStamp()).matrix();
+ Eigen::Matrix<double, 3, 4> P1 = sen_cam_->getProjectionMatrix() * Tcw1;
+ Eigen::Matrix<double, 3, 4> P2 = sen_cam_->getProjectionMatrix() * Tcw2;
+ cv::Mat P1_cv, P2_cv;
+ cv::eigen2cv(P1, P1_cv);
+ cv::eigen2cv(P2, P2_cv);
+ // WOLF_INFO(P1)
+ // WOLF_INFO(P1_cv)
+
+ // perform triangulation of one landmark
+ cv::Vec4d ptcv_w; // output: homogeneous landmark point expressed in world frame
+ cv::triangulatePoints(P1_cv, P2_cv, pix1, pix2, ptcv_w);
+ // WOLF_INFO("YAY: ", ptcv_w)
+
+
+ /////////////////////////////////////////////////////////
+ // check that triangulation was done with enough parallax
+ /////////////////////////////////////////////////////////
+ bool triangulation_is_a_success = true; // Not implemented yet
+ if(!triangulation_is_a_success)
+ {
+ return nullptr;
+ }
+
+ // normalize to make equivalent to a unit quaternion
+ Eigen::Vector4d pt_c;
+ cv::cv2eigen(ptcv_w, pt_c);
+
+ // HACK: to avoid "nans" in residal, set Z = 1
+ pt_c(2) = 1 * pt_c(3);
+
+ // Normilization necessary since homogeneous point stateblock is supposed to be unitary
+ pt_c.normalize();
+
+ WOLF_INFO("New lmk: ", pt_c.transpose())
+ auto lmk_hp_ptr = LandmarkBase::emplace<LandmarkHp>(getProblem()->getMap(),
+ pt_c,
+ feat_pi2->getKeyPoint().getDescriptor());
+
+ // Set all IDs equal to track ID
+ size_t track_id = _feat->trackId();
+ lmk_hp_ptr->setId(track_id);
+ _feat->setLandmarkId(track_id);
+
+ return lmk_hp_ptr;
+}
+
+
LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkNaive(FeatureBasePtr _feat)
{
// Taken from processor_bundle_adjustment
@@ -410,6 +490,24 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkNaive(FeatureBasePtr _fe
}
+
+Eigen::Isometry3d ProcessorVisualOdometry::getTcw(TimeStamp _ts) const
+{
+ // get robot position and orientation at capture's timestamp
+ const auto& state = getProblem()->getState(_ts, "PO");
+ const auto& pos = state.at('P');
+ const auto& ori = state.at('O');
+
+ // compute world-to-camera transform
+ Eigen::Isometry3d T_w_r = Translation3d(pos) * Quaterniond(ori.data());
+ Eigen::Isometry3d T_r_c = Translation3d(getSensor()->getP()->getState())
+ * Quaterniond(getSensor()->getP()->getState().data());
+
+ // invert transform to camera-to-world and return
+ return (T_w_r * T_r_c).inverse();
+}
+
+
void ProcessorVisualOdometry::postProcess()
{
// Delete tracks with no keyframes