Joan Vallvé Navarro
--- a/include/publisher_vision.h 0 → 100644
+++ b/include/publisher_vision.h 0 → 100644
+            Function that change an image to show tracks and features within it
+        */
+        void showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBasePtr _cap_img, COLORFEATURES _color_of_features, bool _show_track_ID);
+
+        Eigen::Matrix<double, 2, 1> getPixelLandmark(LandmarkBasePtr _lmk, CaptureBasePtr _capture);
+        Eigen::Matrix<double, 3, 4> getProjectionMatrix(CaptureBasePtr _capture);
+        Eigen::Matrix<double, 4, 4> getT_c_w(CaptureBasePtr _capture);
+
+
+    protected:
+        ProcessorVisualOdometryPtr processor_vision_;
+        CaptureBasePtr last_capture_;
+
+    public:
+        image_transport::Publisher publisher_image_;
+        image_transport::ImageTransport it_;
--- a/src/publisher_vision.cpp 0 → 100644
+++ b/src/publisher_vision.cpp 0 → 100644
+    // Get capture image
+    auto cap_img = std::dynamic_pointer_cast<CaptureImage>(processor_vision_->getLast());
+
+    assert(cap_img != nullptr && "Received Capture is not of type CaptureImage!");
+
+    auto track_matrix = processor_vision_->getTrackMatrix(); // copy track matrix
+
+    // Draw and publish debug image
+    try
+    {
+        // Extract cv image
+        cv::Mat cv_img_debug;
+        cv::cvtColor(cap_img->getImage(), cv_img_debug, cv::COLOR_GRAY2BGR);
+
+        // Draw all tracks
+        bool show_track_ids = false;
--- a/src/publisher_vision.cpp 0 → 100644
+++ b/src/publisher_vision.cpp 0 → 100644
+        default:
+            break;
+    }
+
+    return color;
+}
+
+void PublisherVisionDebug::showTracks(cv::Mat _image,
+                                      const TrackMatrix &_track_matrix,
+                                      CaptureBasePtr _cap_img,
+                                      COLORFEATURES _color_of_features,
+                                      bool _show_track_ID)
+{
+
+    int size_feature_kf_pix_ = 1;
+    int size_feature_current_pix_ = 2;
--- a/src/publisher_vision.cpp 0 → 100644
+++ b/src/publisher_vision.cpp 0 → 100644
+            std::cout << "SIZE 0 TRACK!!" << '\n';
+            continue;
+        }
+
+        // determine color and luminosity of track
+        std::vector<int> color = colorTrackAndFeatures(track.size(),
+                                                       100, 450,
+                                                       max_feature_in_track,
+                                                       min_feature_in_track,
+                                                       _color_of_features);
+
+        // iterator to current feature in track
+        Track::iterator ftr_current (track.find(time_capture));
+
+        // draw current feature
+        int thickness_curr = 0.1;  // -1 -> filled empty