Joan Vallvé Navarro · bb29bcfe
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
+  const auto& tracks_origin               = _last->getTracksOrigin();
+  const auto& kps_last                    = _last->getKeyPoints();
+  const auto& kps_origin                  = _origin->getKeyPoints();
+
+  for(auto it = tracks_origin.begin(); it != tracks_origin.end(); it++)
+  {
+    
+    const auto& it_ftr_origin    = kps_origin.find(it->first);
+    const auto& it_ftr_last      = kps_last.find(it->second);
+
+    if (it_ftr_origin != kps_origin.end() && it_ftr_last != kps_last.end())
+    {
+      const auto& ftr_origin     = it_ftr_origin->second.getCvKeyPoint();
+      const auto& ftr_last       = it_ftr_last->second.getCvKeyPoint();
+
+      const auto& pos_ftr_origin = std::make_tuple(ftr_origin.pt.x, ftr_origin.pt.y) ;