diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index 83230cac9c0a21dfdae01d22ba9a6d442a447d1e..8789a01e4509f0ff529b6277c16fdda37733fe6d 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -107,7 +107,7 @@ class PublisherVisionDebug : public Publisher
         */
         std::vector<int> colorTrackAndFeatures(int _nb_feature_in_track, int _max_feature_in_tracks, int _min_feature_in_tracks, COLOR _color_of_features);
 
-        std::vector<int> colorLandmarks(COLOR _color_of_lmks);
+        std::vector<int> primaryColor(COLOR _color_of_lmks);
 
         /*
             Change an image to show tracks and features within it
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 15e6721a70d96905eb696f44792dc65dd10cf2f1..95d2c54e1b5b5ff754c2170b3953c7aaeb4fe36b 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -83,8 +83,8 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
     thickness_lmk_                        = getParamWithDefault<double>(_server, prefix_ + "/thickness_lmk", 5);
     size_text_ID_                         = getParamWithDefault<double>(_server, prefix_ + "/size_text_ID", 0.5);
     std::string str_color_tracks_features = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features", "CYAN");
-    std::string str_color_landmarks            = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
-    std::string str_color_last_keypoints            = getParamWithDefault<std::string>(_server, prefix_ + "/str_color_last_keypoints", "CYAN");
+    std::string str_color_landmarks       = getParamWithDefault<std::string>(_server, prefix_ + "/color_landmarks", "CYAN");
+    std::string str_color_last_keypoints  = getParamWithDefault<std::string>(_server, prefix_ + "/str_color_last_keypoints", "CYAN");
     
     // set color attributes
     color_tracks_features_ = colorStringToEnum(str_color_tracks_features);
@@ -229,14 +229,6 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
         color[2] = 0;
     }
 
-//    std::cout << "color HERE " << '\n';
-//    std::cout << "alpha: " << alpha << '\n';
-//    for (int c : color)
-//    {
-//        std::cout << c << " ";
-//    }
-//    std::cout << std::endl;
-
     switch (_color_of_features)
     {
         case BLUE:
@@ -271,11 +263,11 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
     return color;
 }
 
-std::vector<int> PublisherVisionDebug::colorLandmarks(COLOR _color_of_lmks)
+std::vector<int> PublisherVisionDebug::primaryColor(COLOR _color)
 {
   std::vector<int> color = {0, 0, 0};
 
-  switch (_color_of_lmks)
+  switch (_color)
     {
         case BLUE:
             color[2] = 255;
@@ -346,12 +338,14 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
         // iterator to current feature in track
         Track::iterator ftr_current (track.find(time_capture));
 
+        std::vector<int> color_last_kpt = primaryColor(color_last_keypoints_);
+
         // draw current feature
         cv::circle(_image,
                    cv::Point(ftr_current->second->getMeasurement(0),
                              ftr_current->second->getMeasurement(1)),
                    size_feature_curr_pix_,
-                   CV_RGB(color[0], color[1], color[2]),
+                   CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]),
                    thickness_curr_ftr_);
 
         // draw track ID close to current feature
@@ -497,7 +491,7 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
                                          const CaptureBasePtr& _capture)
 {
     // get stuff common to all landmarks
-    std::vector<int> colorLmks = colorLandmarks(color_landmarks_);
+    std::vector<int> colorLmks = primaryColor(color_landmarks_);
 
     const auto& Tcw         = getTcwMatrix(_capture);
     const auto& prj_mat     = getProjectionMatrix(_capture);