Resolve "Publisher for visual odometry"

Closes #1 (closed)

include/publisher_vision.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef PUBLISHER_VISION_DEBUG_H_
+#define PUBLISHER_VISION_DEBUG_H_
+
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include "core/common/wolf.h"
+#include "vision/processor/processor_visual_odometry.h"
+#include <core/utils/params_server.h>
+
+#include "publisher.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <ros/ros.h>
+#include <sensor_msgs/Image.h>
+#include <cv_bridge/cv_bridge.h>
+#include <image_transport/image_transport.h>
+
+
+
+namespace wolf
+{
+
+enum Color {BLUE = 0,
+            GREEN, 
+            YELLOW, 
+            MAGENTA, 
+            CYAN, 
+            GREY,
+            RED
+};
+class PublisherVisionDebug : public Publisher
+{
+    public:
+        PublisherVisionDebug(const std::string& _unique_name,
+                             const ParamsServer& _server,
+                             const ProblemConstPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherVisionDebug);
+
+        virtual ~PublisherVisionDebug(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        bool ready() override;
+        void publishDerived() override;
+
+    protected:
+        ProcessorVisualOdometryConstPtr processor_vision_;
+        CaptureBaseConstPtr             last_capture_;
+        std::string                     topic_preprocessor_;
+
+        struct Feature {
+            double  thickness_;
+            int     size_pix_;
+            Color   color_;
+        }; 
+        
+        struct Tracks {
+            bool    show_id_;
+            double  size_id_;
+            double  thickness_;
+            bool    min_max_on_alive_tracks_;
+            Color   color_;
+            int     min_luminosity_;
+            int     max_luminosity_;
+            Feature feature_last_;
+            Feature feature_kfs_;
+        };
+        struct Tracks_preprocess {
+            bool    show_;
+            bool    show_on_diff_topic_;
+            bool    show_features_;
+            double  thickness_;
+            Color   color_;
+        };
+        struct Landmarks {
+            bool    show_id_;
+            double  size_id_;
+            int     size_pix_;
+            Color   color_;
+        };
+
+        Tracks            tracks_;
+        Tracks_preprocess tracks_preprocess_;
+        Landmarks         landmarks_;
+
+        image_transport::Publisher      publisher_image_;
+        image_transport::Publisher      publisher_image_preprocess_;
+        image_transport::ImageTransport img_transport_;
+
+    private:
+        cv::Scalar primaryColor(Color _color);
+        cv::Scalar colorTrackAndFeatures(int _nb_feature_in_track,
+                                                             int _max_feature_in_tracks,
+                                                             int _min_feature_in_tracks,
+                                                             Color _color_of_features);
+        /*
+            Convert a string corresponding to a color to its corresponding Color enum value
+        */
+        Color                       colorStringToEnum(const std::string _color);
+
+        /*
+            Search for the maximum and minimum of features in one track in the track matrix
+        */
+        std::pair<int, int>         minMaxAliveTrackLength(const TrackMatrix &_track_matrix, CaptureBaseConstPtr _cap_img) const;
+
+        /*
+            Return a unique RGB color vector depending on the lenght of the track given,
+            so the longer is the track the darker it is
+        */
+        // std::vector<int>            colorTrackAndFeatures(int _nb_feature_in_track, int _max_feature_in_tracks, int _min_feature_in_tracks, Color _color_of_features);
+        
+        /*
+            Return a  RGB color vector coresponding to the color asked
+        */
+        // std::vector<int>            primaryColor(Color _color_of_lmks);
+
+        /*
+            Change an image to show tracks and features within it
+        */
+        void                        showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBaseConstPtr _cap_img);
+        
+        /*
+            Return the transformation from world to camera
+        */
+        Eigen::Isometry3d           getTcw               (const CaptureBaseConstPtr _capture) const;
+
+        /*
+            Transform Tcw into matrix for multiplication purpose
+        */
+        Eigen::Matrix4d             getTcwMatrix         (const CaptureBaseConstPtr _capture) const;
+        
+        /*
+            Return the projection matrix of a sensor (linked to a capture)
+        */
+        Eigen::Matrix<double, 3, 4> getProjectionMatrix  (const CaptureBaseConstPtr _capture) const;
+
+        /*
+            Return only the landmark associated to a feature in last keyframes
+        */
+        LandmarkBaseConstPtr        getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBaseConstPtr& _ftr);
+
+        /*
+            Return the projection of a landmark in the frame
+        */
+        Eigen::Vector2d             projectLandmarkHp    (const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBaseConstPtr _lmk);
+        
+        /*
+            Print one landmark into the capture
+        */
+        void                        showLandmark         (cv::Mat _image, const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
+        
+        /*
+            Print all of the landmarks associated with the capture
+        */
+        void                        showLandmarks        (cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBaseConstPtr& _cap);
+
+        /*
+            Change an image to show tracks and features from the preprocess within it
+        */
+        void                        showTracksPreprocess (cv::Mat _image, const CaptureImageConstPtr& _origin, const CaptureImageConstPtr& _last);
+};
+
+WOLF_REGISTER_PUBLISHER(PublisherVisionDebug)
+}
+
+#endif