Fix constness, add method, order class declaration

include/publisher_vision.h

@@ -56,6 +56,26 @@ enum COLORFEATURES {BLUE = 0,
 
 class PublisherVisionDebug : public Publisher
 {
+    public:
+        PublisherVisionDebug(const std::string& _unique_name,
+                             const ParamsServer& _server,
+                             const ProblemPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherVisionDebug);
+
+        virtual ~PublisherVisionDebug(){};
+
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+
+        bool ready() override;
+        void publishDerived() override;
+
+    protected:
+        ProcessorVisualOdometryPtr processor_vision_;
+        CaptureBasePtr last_capture_;
+
+        image_transport::Publisher publisher_image_;
+        image_transport::ImageTransport img_transport_;
+
     private:
         /*
             Function that search for the maximum and minimum of features in one track in the track matrix
@@ -73,29 +93,11 @@ class PublisherVisionDebug : public Publisher
         */
         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);
+        Eigen::Vector2d             getPixelLandmark(const LandmarkBasePtr _lmk, const CaptureBasePtr _capture) const;
+        Eigen::Matrix<double, 3, 4> getProjectionMatrix(const CaptureBasePtr _capture) const;
+        Eigen::Isometry3d           getTcw(const CaptureBasePtr _capture) const;
+        Eigen::Matrix4d             getTcwMatrix(const CaptureBasePtr _capture) const;
 
-
-    protected:
-        ProcessorVisualOdometryPtr processor_vision_;
-        CaptureBasePtr last_capture_;
-
-    public:
-        image_transport::Publisher publisher_image_;
-        image_transport::ImageTransport it_;
-        PublisherVisionDebug(const std::string& _unique_name,
-                               const ParamsServer& _server,
-                               const ProblemPtr _problem);
-        WOLF_PUBLISHER_CREATE(PublisherVisionDebug);
-
-        virtual ~PublisherVisionDebug(){};
-
-        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
-
-        bool ready() override;
-        void publishDerived() override;
 };
 
 WOLF_REGISTER_PUBLISHER(PublisherVisionDebug)

src/publisher_vision.cpp

@@ -45,7 +45,7 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
         Publisher(_unique_name, _server, _problem),
         processor_vision_(nullptr),
         last_capture_(nullptr),
-        it_(ros::NodeHandle())
+        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", "");
@@ -70,8 +70,8 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
 
 void PublisherVisionDebug::initialize(ros::NodeHandle &nh, const std::string &topic)
 {
-    it_ = image_transport::ImageTransport(nh);
-    publisher_image_ = it_.advertise(topic, 10);
+    img_transport_ = image_transport::ImageTransport(nh);
+    publisher_image_ = img_transport_.advertise(topic, 10);
 }
 
 bool PublisherVisionDebug::ready()
@@ -215,9 +215,12 @@ std::vector<int> PublisherVisionDebug::colorTrackAndFeatures(int _nb_feature_in_
             color[2] = color[0];
             break;
         case CYAN:
+        {
+            int col_tmp = color[2];
             color[2] = color[0];
             color[0] = color[1];
-            color[1] = color[2];
+            color[1] = col_tmp;
+        }
             break;
         case GREY:
             color[2] = color[0];
@@ -320,23 +323,25 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
     }
 }
 
-Eigen::Matrix<double, 4, 4> PublisherVisionDebug::getT_c_w(CaptureBasePtr _capture)
+Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBasePtr _capture) const
 {
     auto state = _capture->getProblem()->getState(_capture->getTimeStamp());
     auto pos = state['P'];
     auto orientation = state['O'].data();
 
-    Transform<double, 3, Isometry> T_w_r = Translation<double, 3>(pos) * Quaterniond(orientation);
-    Transform<double, 3, Isometry> T_r_c = Translation<double, 3>(_capture->getSensorP()->getState())
+    Eigen::Isometry3d T_w_r = Translation<double, 3>(pos) * Quaterniond(orientation);
+    Eigen::Isometry3d T_r_c = Translation<double, 3>(_capture->getSensorP()->getState())
             * Quaterniond(_capture->getSensorO()->getState().data());
 
-    Transform<double, 3, Isometry> T_c_w = (T_w_r * T_r_c).inverse();
-    Eigen::Matrix<double, 4, 4> T_c_w_Matrix(T_c_w.matrix());
+    return (T_w_r * T_r_c).inverse();
+}
 
-    return T_c_w_Matrix;
+Eigen::Matrix4d PublisherVisionDebug::getTcwMatrix(const CaptureBasePtr _capture) const
+{
+    return getTcw(_capture).matrix();
 }
 
-Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(CaptureBasePtr _capture)
+Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const CaptureBasePtr _capture) const
 {
     auto k(_capture->getSensor()->getIntrinsic()->getState());
     Eigen::Matrix<double, 3, 4> P;
@@ -346,12 +351,12 @@ Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(CaptureBas
     return P;
 }
 
-Eigen::Matrix<double, 2, 1> PublisherVisionDebug::getPixelLandmark(LandmarkBasePtr _lmk, CaptureBasePtr _capture)
+Eigen::Vector2d PublisherVisionDebug::getPixelLandmark(const LandmarkBasePtr _lmk, const CaptureBasePtr _capture) const
 {
     auto pos = _lmk->getP()->getState();
-    Eigen::Matrix<double, 3, 1> pixel_position3 = getProjectionMatrix(_capture) * getT_c_w(_capture) * pos;
+    Eigen::Vector3d pixel_position3 = getProjectionMatrix(_capture) * getTcwMatrix(_capture) * pos;
 
-    Eigen::Matrix<double, 2, 1> pixel_position;
+    Eigen::Vector2d pixel_position;
     pixel_position << pixel_position3(0) / pixel_position3(2), pixel_position3(1) / pixel_position3(2);
 
     return pixel_position;