[skip-ci] Implement new triangulation

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.
          *

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