diff --git a/include/publisher_vision.h b/include/publisher_vision.h
index d278e777a60d5198ed06c7ae1f2ee0a22f23bb9d..1eb4e687b7ea5a8e13916b25dcf140085414f77e 100644
--- a/include/publisher_vision.h
+++ b/include/publisher_vision.h
@@ -53,7 +53,6 @@ enum COLOR {BLUE = 0,
             GREY,
             RED
 };
-
 class PublisherVisionDebug : public Publisher
 {
     public:
@@ -73,21 +72,36 @@ class PublisherVisionDebug : public Publisher
         ProcessorVisualOdometryPtr processor_vision_;
         CaptureBasePtr last_capture_;
 
-        bool show_tracks_ID_;
-        bool show_landmarks_ID_;
-        int min_luminosity_;
-        int max_luminosity_;
-        double thickness_curr_ftr_;
-        int size_feature_curr_pix_;
-        double thickness_kfs_ftr_;
-        int size_feature_kfs_pix_;
-        double thickness_lmk_;
-        double size_text_ID_;
-        double thickness_track_;
-        COLOR color_tracks_features_;
-        COLOR color_tracks_features_preprocess_;
-        COLOR color_landmarks_;
-        COLOR color_last_keypoints_;
+        struct Feature {
+                double thickness_;
+                int size_pix_;
+                COLOR color_;
+        }; 
+        
+        struct Tracks {
+            bool show_id_;
+            int size_id_;
+            double thickness_;
+            COLOR color_;
+            int min_luminosity_;
+            int max_luminosity_;
+            struct Feature feature_last_;
+            struct Feature feature_kfs_;
+        };
+        struct Tracks_preprocess {
+            double thickness_;
+            COLOR color_;
+        };
+        struct Landmarks {
+            bool show_id_;
+            int size_id_;
+            int size_pix_;
+            COLOR color_;
+        };
+
+        Tracks tracks_;
+        Tracks_preprocess tracks_preprocess_;
+        Landmarks landmarks_;
 
         image_transport::Publisher publisher_image_;
         image_transport::ImageTransport img_transport_;
diff --git a/src/publisher_vision.cpp b/src/publisher_vision.cpp
index 2345c3e4401c145f394b6bedb707fe802dd9e593..ee68932038b6d02a5eb998c02004ca7965ab7fee 100644
--- a/src/publisher_vision.cpp
+++ b/src/publisher_vision.cpp
@@ -70,29 +70,34 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
         img_transport_(ros::NodeHandle())
 {
     // if user do not provide processor's name, first processor of type PublisherVisionDebug is taken
-    auto processor_name                   = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", "");
-
-    show_tracks_ID_                                  = getParamWithDefault<bool>(_server, prefix_ + "/show_tracks_ID", false);
-    show_landmarks_ID_                               = getParamWithDefault<bool>(_server, prefix_ + "/show_landmarks_ID", false);
-    min_luminosity_                                  = getParamWithDefault<int>(_server, prefix_ + "/min_luminosity", 100);
-    max_luminosity_                                  = getParamWithDefault<int>(_server, prefix_ + "/max_luminosity", 450);
-    thickness_curr_ftr_                              = getParamWithDefault<double>(_server, prefix_ + "/thickness_curr_ftr", -1);
-    size_feature_curr_pix_                           = getParamWithDefault<int>(_server, prefix_ + "/size_feature_curr_pix", 2);
-    thickness_kfs_ftr_                               = getParamWithDefault<double>(_server, prefix_ + "/thickness_kfs_ftr", 0.1);
-    size_feature_kfs_pix_                            = getParamWithDefault<int>(_server, prefix_ + "/size_feature_kfs_pix", 1);
-    thickness_lmk_                                   = getParamWithDefault<double>(_server, prefix_ + "/thickness_lmk", 5);
-    size_text_ID_                                    = getParamWithDefault<double>(_server, prefix_ + "/size_text_ID", 0.5);
-    thickness_track_                                 = getParamWithDefault<double>(_server, prefix_ + "/thickness_track", 1.5);
-    std::string str_color_tracks_features            = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features", "CYAN");
-    std::string str_color_tracks_features_preProcess = getParamWithDefault<std::string>(_server, prefix_ + "/color_tracks_features_preprocess", "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_ + "/color_last_keypoints", "CYAN");
-    
-    // set color attributes
-    color_tracks_features_            = colorStringToEnum(str_color_tracks_features);
-    color_tracks_features_preprocess_ = colorStringToEnum(str_color_tracks_features);
-    color_landmarks_                  = colorStringToEnum(str_color_landmarks);
-    color_last_keypoints_             = colorStringToEnum(str_color_last_keypoints);
+    auto processor_name              = getParamWithDefault<std::string>(_server, prefix_ + "/processor_name", "");
+
+    //Tracks
+    tracks_.show_id_                 = getParamWithDefault<bool>(_server, prefix_ + "/tracks/show_id", false);
+    tracks_.size_id_                 = getParamWithDefault<int>(_server, prefix_ + "/tracks/size_id", 0.5);
+    tracks_.thickness_               = getParamWithDefault<double>(_server, prefix_ + "/tracks/thickness", 1.5);
+    std::string str_color_tracks     = getParamWithDefault<std::string>(_server, prefix_ + "/tracks/color", "CYAN");
+    tracks_.color_                   = colorStringToEnum(str_color_tracks);
+    tracks_.min_luminosity_          = getParamWithDefault<int>(_server, prefix_ + "/tracks/min_lum", 100);
+    tracks_.max_luminosity_          = getParamWithDefault<int>(_server, prefix_ + "/tracks/max_lum", 450);
+    tracks_.min_luminosity_          = (tracks_.min_luminosity_ < 0   ? 0   : tracks_.min_luminosity_);
+    tracks_.max_luminosity_          = (tracks_.max_luminosity_ > 510 ? 510 : tracks_.max_luminosity_);
+    //Feature_last
+    tracks_.feature_last_.thickness_ = getParamWithDefault<double>(_server, prefix_ + "/tracks/feature_last/thickness", -1);
+    tracks_.feature_last_.size_pix_  = getParamWithDefault<int>(_server, prefix_ + "/tracks/feature_last/size_pix", 2);
+    std::string str_color_ftr_last   = getParamWithDefault<std::string>(_server, prefix_ + "/tracks/feature_last/color", "RED");
+    tracks_.feature_last_.color_     = colorStringToEnum(str_color_ftr_last);
+    //Feature_kfs
+    tracks_.feature_kfs_.thickness_  = getParamWithDefault<double>(_server, prefix_ + "/tracks/feature_kfs/thickness", 0.1);
+    tracks_.feature_kfs_.size_pix_   = getParamWithDefault<int>(_server, prefix_ + "/tracks/feature_kfs/size_pix", 1);
+
+    //Landmarks
+    landmarks_.show_id_              = getParamWithDefault<bool>(_server, prefix_ + "/landmarks/show_id", false);
+    landmarks_.size_id_              = getParamWithDefault<int>(_server, prefix_ + "/landmarks/size_id", 0.5);
+    landmarks_.size_pix_             = getParamWithDefault<int>(_server, prefix_ + "/landmarks/size_pix", 5);
+    std::string str_color_landmarks_ = getParamWithDefault<std::string>(_server, prefix_ + "/landmarks/color", "CYAN");
+    landmarks_.color_                = colorStringToEnum(str_color_landmarks_);
+
 
     // search the processor
     if (processor_name == "")
@@ -232,7 +237,7 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
 //        alpha = 0.5; // put "half luminosity as a default value"
     }
 
-    double lum = (min_luminosity_ + (max_luminosity_ - min_luminosity_) * (1 - alpha));
+    double lum = (tracks_.min_luminosity_ + (tracks_.max_luminosity_ - tracks_.min_luminosity_) * (1 - alpha));
     if (lum > 255)
     {
         color[0] = 255;
@@ -347,26 +352,27 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
         }
 
         // determine color and luminosity of track
-        std::vector<int> color = colorTrackAndFeatures(track.size(),
-                                                       max_feature_in_track,
-                                                       min_feature_in_track,
-                                                       color_tracks_features_);
+        std::vector<int> color          = colorTrackAndFeatures(track.size(),
+                                                                max_feature_in_track,
+                                                                min_feature_in_track,
+                                                                tracks_.color_);
+
+        std::vector<int> color_last_kpt = primaryColor(tracks_.feature_last_.color_);
+
 
         // 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_,
+                   tracks_.feature_last_.size_pix_,
                    CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]),
-                   thickness_curr_ftr_);
+                   tracks_.feature_last_.thickness_);
 
         // draw track ID close to current feature
-        if (show_tracks_ID_) //show features ID of current frame
+        if (tracks_.show_id_) //show features ID of current frame
         {
             int shift_text_x = 7;
             int shift_text_y = 10;
@@ -375,7 +381,8 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
                     std::to_string(ftr_current->second->trackId()),
                     cv::Point(ftr_current->second->getMeasurement(0) - shift_text_x,
                               ftr_current->second->getMeasurement(1) - shift_text_y),
-                    1, size_text_ID_, CV_RGB(color[0], color[1], color[2]));
+                    1, tracks_.size_id_, 
+                    CV_RGB(color_last_kpt[0], color_last_kpt[1], color_last_kpt[2]));
         }
 
 
@@ -391,13 +398,19 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
             {
                 // mark features of previous keyframes
                 cv::circle(_image, cv::Point(ftr->second->getMeasurement(0), ftr->second->getMeasurement(1)),
-                           size_feature_kfs_pix_, CV_RGB(color[0], color[1], color[2]), thickness_kfs_ftr_);
+                           tracks_.feature_kfs_.size_pix_, 
+                           CV_RGB(color[0], color[1], color[2]), 
+                           tracks_.feature_kfs_.thickness_);
             }
 
             // draw line in between keyframes (as well as last KF and current frame)
-            cv::line(_image, cv::Point(ftr->second->getMeasurement(0), ftr->second->getMeasurement(1)),
-                     cv::Point(previous->second->getMeasurement(0), previous->second->getMeasurement(1)),
-                     CV_RGB(color[0], color[1], color[2]), thickness_track_);
+            cv::line(_image, 
+                     cv::Point(ftr->second->getMeasurement(0), 
+                               ftr->second->getMeasurement(1)),
+                     cv::Point(previous->second->getMeasurement(0), 
+                               previous->second->getMeasurement(1)),
+                     CV_RGB(color[0], color[1], color[2]), 
+                     tracks_.thickness_);
         }
 
     }
@@ -461,54 +474,54 @@ Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<doub
 
   Eigen::Vector2d pixel;
   pixel << pixel_hmg(0)/pixel_hmg(2),
-               pixel_hmg(1)/pixel_hmg(2);
+           pixel_hmg(1)/pixel_hmg(2);
 
   return pixel;
 }
 
-void PublisherVisionDebug::showLandmark(cv::Mat _image,
-                                        const TrackMatrix& _track_matrix,
-                                        const FeatureBasePtr& _ftr)
-{
-
-    // get stuff common to all landmarks
-    const auto& capture     = _ftr->getCapture();
-    const auto& Tcw         = getTcwMatrix(capture);
-    const auto& prj_mat     = getProjectionMatrix(capture);
-    const auto& trf_prj_mat = prj_mat * Tcw;
-
-    // get stuff of this landmark
-    const auto& lmk         = getAssociatedLandmark(_track_matrix, _ftr);
-    const auto& lmk_pixel   = projectLandmarkHp(trf_prj_mat, lmk);
-
-    if (lmk != nullptr)
-    {
-        cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
-                       CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
-                       thickness_lmk_, 1);
-
-        if (show_landmarks_ID_)
-        {
-            unsigned int lmk_id = lmk->id();
-            int shift_text_x = 7;
-            int shift_text_y = 10;
-
-            cv::putText(
-                    _image,
-                    std::to_string(lmk_id),
-                    cv::Point(lmk_pixel(0) - shift_text_x,
-                              lmk_pixel(1) - shift_text_y),
-                              1, size_text_ID_, CV_RGB(0, 0, 0));
-        }
-    }
-}
+// void PublisherVisionDebug::showLandmark(cv::Mat _image,
+                                        // const TrackMatrix& _track_matrix,
+                                        // const FeatureBasePtr& _ftr)
+// {
+
+    // // get stuff common to all landmarks
+    // const auto& capture     = _ftr->getCapture();
+    // const auto& Tcw         = getTcwMatrix(capture);
+    // const auto& prj_mat     = getProjectionMatrix(capture);
+    // const auto& trf_prj_mat = prj_mat * Tcw;
+
+    // // get stuff of this landmark
+    // const auto& lmk         = getAssociatedLandmark(_track_matrix, _ftr);
+    // const auto& lmk_pixel   = projectLandmarkHp(trf_prj_mat, lmk);
+
+    // if (lmk != nullptr)
+    // {
+        // cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
+                      //  CV_RGB(0, 0, 0), cv::MARKER_TILTED_CROSS,
+                      //  thickness_lmk_, 1);
+
+        // if (show_landmarks_ID_)
+        // {
+            // unsigned int lmk_id = lmk->id();
+            // int shift_text_x = 7;
+            // int shift_text_y = 10;
+
+            // cv::putText(
+                    // _image,
+                    // std::to_string(lmk_id),
+                    // cv::Point(lmk_pixel(0) - shift_text_x,
+                              // lmk_pixel(1) - shift_text_y),
+                              // 1, size_text_ID_, CV_RGB(0, 0, 0));
+        // }
+    // }
+// }
 
 void PublisherVisionDebug::showLandmarks(cv::Mat _image,
                                          const TrackMatrix& _track_matrix,
                                          const CaptureBasePtr& _capture)
 {
     // get stuff common to all landmarks
-    std::vector<int> colorLmks = primaryColor(color_landmarks_);
+    std::vector<int> color_lmks = primaryColor(landmarks_.color_);
 
     const auto& Tcw         = getTcwMatrix(_capture);
     const auto& prj_mat     = getProjectionMatrix(_capture);
@@ -526,73 +539,72 @@ void PublisherVisionDebug::showLandmarks(cv::Mat _image,
             const auto& lmk_pixel   = projectLandmarkHp(trf_prj_mat, lmk);
 
             cv::drawMarker(_image, cv::Point(lmk_pixel(0), lmk_pixel(1)),
-                           CV_RGB(colorLmks[0], colorLmks[1], colorLmks[2]), 
+                           CV_RGB(color_lmks[0], color_lmks[1], color_lmks[2]), 
                            cv::MARKER_TILTED_CROSS,
-                           5, 1);
+                           landmarks_.size_pix_, 1);
 
-            if (show_landmarks_ID_)
+            if (landmarks_.show_id_)
             {
                 unsigned int lmk_id = lmk->id();
                 int shift_text_x = 7;
                 int shift_text_y = 10;
 
-                cv::putText(
-                        _image,
-                        std::to_string(lmk_id),
-                        cv::Point(lmk_pixel(0) - shift_text_x,
-                                  lmk_pixel(1) - shift_text_y),
-                                  1, size_text_ID_, 
-                                  CV_RGB(colorLmks[0], colorLmks[1], colorLmks[2]));
+                cv::putText(_image,
+                            std::to_string(lmk_id),
+                            cv::Point(lmk_pixel(0) - shift_text_x,
+                                      lmk_pixel(1) - shift_text_y),
+                            1, landmarks_.size_id_, 
+                            CV_RGB(color_lmks[0], color_lmks[1], color_lmks[2]));
             }
         }
 
     }
 }
 
-void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
-                                                const CaptureImagePtr& _origin,
-                                                const CaptureImagePtr& _last)
-{
-  std::vector<int> color_track_preprocess = primaryColor(color_tracks_features_preprocess_);
+// void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
+                                                // const CaptureImagePtr& _origin,
+                                                // const CaptureImagePtr& _last)
+// {
+  // std::vector<int> color_track_preprocess = primaryColor(color_tracks_features_preprocess_);
 
-  const auto& tracks_origin               = _last->getTracksOrigin();
-  const auto& kps_last                    = _last->getKeyPoints();
-  const auto& kps_origin                  = _origin->getKeyPoints();
+  // 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++)
-  {
+  // 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) ;
-      const auto& pos_ftr_last   = std::make_tuple(ftr_last.pt.x, ftr_last.pt.y);
-
-      cv::circle(_image,
-                cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
-                size_feature_curr_pix_,
-                CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
-                thickness_curr_ftr_);
+    // 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) ;
+      // const auto& pos_ftr_last   = std::make_tuple(ftr_last.pt.x, ftr_last.pt.y);
+
+      // cv::circle(_image,
+                // cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
+                // size_feature_curr_pix_,
+                // CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+                // thickness_curr_ftr_);
       
-      cv::circle(_image,
-                cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
-                size_feature_kfs_pix_,
-                CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
-                thickness_kfs_ftr_);
-
-      cv::line(_image, 
-              cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
-              cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
-              CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
-              thickness_track_);
-    }
-  }
-}
+      // cv::circle(_image,
+                // cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
+                // size_feature_kfs_pix_,
+                // CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+                // thickness_kfs_ftr_);
+
+      // cv::line(_image, 
+              // cv::Point(std::get<0>(pos_ftr_last), std::get<1>(pos_ftr_last)),
+              // cv::Point(std::get<0>(pos_ftr_origin), std::get<1>(pos_ftr_origin)),
+              // CV_RGB(color_track_preprocess[0], color_track_preprocess[1], color_track_preprocess[2]),
+              // thickness_track_);
+    // }
+  // }
+// }
 
 
 }