[slip-ci] adapt to constness changes

include/publisher_vision.h

@@ -58,7 +58,7 @@ class PublisherVisionDebug : public Publisher
     public:
         PublisherVisionDebug(const std::string& _unique_name,
                              const ParamsServer& _server,
-                             const ProblemPtr _problem);
+                             const ProblemConstPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherVisionDebug);
 
         virtual ~PublisherVisionDebug(){};
@@ -69,9 +69,9 @@ class PublisherVisionDebug : public Publisher
         void publishDerived() override;
 
     protected:
-        ProcessorVisualOdometryPtr processor_vision_;
-        CaptureBasePtr             last_capture_;
-        std::string                topic_preprocessor_;
+        ProcessorVisualOdometryConstPtr processor_vision_;
+        CaptureBaseConstPtr             last_capture_;
+        std::string                     topic_preprocessor_;
 
         struct Feature {
             double  thickness_;
@@ -126,7 +126,7 @@ class PublisherVisionDebug : public Publisher
         /*
             Search for the maximum and minimum of features in one track in the track matrix
         */
-        std::pair<int, int>         minMaxAliveTrackLength(const TrackMatrix &_track_matrix, CaptureBasePtr _cap_img) const;
+        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,
@@ -142,32 +142,32 @@ class PublisherVisionDebug : public Publisher
         /*
             Change an image to show tracks and features within it
         */
-        void                        showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBasePtr _cap_img);
+        void                        showTracks(cv::Mat _image, const TrackMatrix& _track_matrix, CaptureBaseConstPtr _cap_img);
         
         /*
             Return the transformation from world to camera
         */
-        Eigen::Isometry3d           getTcw               (const CaptureBasePtr _capture) const;
+        Eigen::Isometry3d           getTcw               (const CaptureBaseConstPtr _capture) const;
 
         /*
             Transform Tcw into matrix for multiplication purpose
         */
-        Eigen::Matrix4d             getTcwMatrix         (const CaptureBasePtr _capture) const;
+        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 CaptureBasePtr _capture) const;
+        Eigen::Matrix<double, 3, 4> getProjectionMatrix  (const CaptureBaseConstPtr _capture) const;
 
         /*
             Return only the landmark associated to a feature in last keyframes
         */
-        LandmarkBasePtr             getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr);
+        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 LandmarkBasePtr _lmk);
+        Eigen::Vector2d             projectLandmarkHp    (const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBaseConstPtr _lmk);
         
         /*
             Print one landmark into the capture
@@ -177,12 +177,12 @@ class PublisherVisionDebug : public Publisher
         /*
             Print all of the landmarks associated with the capture
         */
-        void                        showLandmarks        (cv::Mat _image, const TrackMatrix& _track_matrix, const CaptureBasePtr& _cap);
+        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 CaptureImagePtr& _origin, const CaptureImagePtr& _last);
+        void                        showTracksPreprocess (cv::Mat _image, const CaptureImageConstPtr& _origin, const CaptureImageConstPtr& _last);
 };
 
 WOLF_REGISTER_PUBLISHER(PublisherVisionDebug)

src/publisher_vision.cpp

@@ -63,7 +63,7 @@ Color PublisherVisionDebug::colorStringToEnum(const std::string _color)
 
 PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
                                            const ParamsServer &_server,
-                                           const ProblemPtr _problem) :
+                                           const ProblemConstPtr _problem) :
         Publisher(_unique_name, _server, _problem),
         processor_vision_(nullptr),
         last_capture_(nullptr),
@@ -113,11 +113,11 @@ PublisherVisionDebug::PublisherVisionDebug(const std::string &_unique_name,
         throw std::runtime_error(
                 "PublisherVisionDebug: name of processor of type ProcessorVisualOdometry not provided");
 
-    auto proc = problem_->findProcessor(processor_name);
+    ProcessorBaseConstPtr proc = problem_->findProcessor(processor_name);
     if (not proc)
         throw std::runtime_error(
                 "PublisherVisionDebug: processor " + processor_name + " not found.");
-    processor_vision_ = std::dynamic_pointer_cast<ProcessorVisualOdometry>(proc);
+    processor_vision_ = std::dynamic_pointer_cast<const ProcessorVisualOdometry>(proc);
     if (not processor_vision_)
         throw std::runtime_error(
                 "PublisherVisionDebug: processor " + processor_name + " is not of type ProcessorVisualOdometry");
@@ -146,8 +146,8 @@ void PublisherVisionDebug::publishDerived()
         return;
 
     // Get capture image
-    auto cap_img        = std::dynamic_pointer_cast<CaptureImage>(processor_vision_->getLast());
-    auto cap_img_origin = std::dynamic_pointer_cast<CaptureImage>(processor_vision_->getOrigin());
+    auto cap_img        = std::dynamic_pointer_cast<const CaptureImage>(processor_vision_->getLast());
+    auto cap_img_origin = std::dynamic_pointer_cast<const CaptureImage>(processor_vision_->getOrigin());
 
     assert(cap_img          != nullptr && "Received Capture is not of type CaptureImage!");
     assert(cap_img_origin   != nullptr && "Received origin Capture is not of type CaptureImage!");
@@ -211,17 +211,17 @@ void PublisherVisionDebug::publishDerived()
     }
 }
 
-std::pair<int, int> PublisherVisionDebug::minMaxAliveTrackLength(const TrackMatrix &_track_matrix, CaptureBasePtr _cap_img) const
+std::pair<int, int> PublisherVisionDebug::minMaxAliveTrackLength(const TrackMatrix &_track_matrix, CaptureBaseConstPtr _cap_img) const
 {
 
-    map<size_t, FeatureBasePtr> alive_features = _track_matrix.snapshot(_cap_img);
+    SnapshotConst alive_features = _track_matrix.snapshot(_cap_img);
 
     int max = -1;
     int min = 10;
 
     for (auto alive_feat : alive_features)
     {
-        Track track_alive = _track_matrix.track(alive_feat.second->trackId());
+        TrackConst track_alive = _track_matrix.track(alive_feat.second->trackId());
 
         int count = track_alive.size();
 
@@ -357,7 +357,7 @@ cv::Scalar PublisherVisionDebug::primaryColor(Color _color)
 
 void PublisherVisionDebug::showTracks(cv::Mat _image,
                                       const TrackMatrix &_track_matrix,
-                                      CaptureBasePtr _capture_img)
+                                      CaptureBaseConstPtr _capture_img)
 {
     std::pair<int, int> min_max_track_length;
 
@@ -446,7 +446,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
     for (auto ftr_alive : _track_matrix.snapshot(_capture_img))
     {
         // get full track of feature
-        Track track = _track_matrix.track(ftr_alive.second->trackId());
+        TrackConst track = _track_matrix.track(ftr_alive.second->trackId());
         if (track.size() == 0)
         {
             WOLF_WARN("SIZE 0 TRACK!!");
@@ -485,7 +485,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
 
 
         // iterate track from current time to the beginning of track
-        for (Track::reverse_iterator ftr = track.rbegin(); ftr != track.rend(); ++ftr) // start at the feature of the capture to draw
+        for (TrackConst::reverse_iterator ftr = track.rbegin(); ftr != track.rend(); ++ftr) // start at the feature of the capture to draw
         {
             auto previous = std::prev(ftr); //previous iterator
 
@@ -516,7 +516,7 @@ void PublisherVisionDebug::showTracks(cv::Mat _image,
     }
 }
 
-Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBasePtr _capture) const
+Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBaseConstPtr _capture) const
 {
     // get robot position and orientation at capture's timestamp
     const auto& state   = problem_->getState(_capture->getTimeStamp(), "PO");
@@ -532,17 +532,17 @@ Eigen::Isometry3d PublisherVisionDebug::getTcw(const CaptureBasePtr _capture) co
     return (T_w_r * T_r_c).inverse();
 }
 
-Eigen::Matrix4d PublisherVisionDebug::getTcwMatrix(const CaptureBasePtr _capture) const
+Eigen::Matrix4d PublisherVisionDebug::getTcwMatrix(const CaptureBaseConstPtr _capture) const
 {
     return getTcw(_capture).matrix();
 }
 
-Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const CaptureBasePtr _capture) const
+Eigen::Matrix<double, 3, 4> PublisherVisionDebug::getProjectionMatrix(const CaptureBaseConstPtr _capture) const
 {
-    return (std::static_pointer_cast<SensorCamera>(_capture->getSensor()))->getProjectionMatrix();
+    return (std::static_pointer_cast<const SensorCamera>(_capture->getSensor()))->getProjectionMatrix();
 }
 
-LandmarkBasePtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBasePtr& _ftr)
+LandmarkBaseConstPtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _track_matrix, const FeatureBaseConstPtr& _ftr)
 {
     const auto& track_at_kfs      = _track_matrix.trackAtKeyframes(_ftr->trackId());
 
@@ -560,7 +560,7 @@ LandmarkBasePtr PublisherVisionDebug::getAssociatedLandmark(const TrackMatrix& _
     return lmk;
 }
 
-Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBasePtr _lmk)
+Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<double, 3, 4>& _trf_proj_mat, const LandmarkBaseConstPtr _lmk)
 {
     const auto& pos           = _lmk->getP()->getState();
     Eigen::Vector3d pixel_hmg = _trf_proj_mat * pos;
@@ -575,7 +575,7 @@ Eigen::Vector2d PublisherVisionDebug::projectLandmarkHp(const Eigen::Matrix<doub
 
 void PublisherVisionDebug::showLandmarks(cv::Mat                _image,
                                          const TrackMatrix&     _track_matrix,
-                                         const CaptureBasePtr&  _capture)
+                                         const CaptureBaseConstPtr&  _capture)
 {
     // get stuff common to all landmarks
     cv::Scalar color_lmks = primaryColor(landmarks_.color_);
@@ -626,8 +626,8 @@ void PublisherVisionDebug::showLandmarks(cv::Mat                _image,
 }
 
 void PublisherVisionDebug::showTracksPreprocess(cv::Mat _image,
-                                                const CaptureImagePtr& _origin,
-                                                const CaptureImagePtr& _last)
+                                                const CaptureImageConstPtr& _origin,
+                                                const CaptureImageConstPtr& _last)
 {
     cv::Scalar color_track_preprocess = primaryColor(tracks_preprocess_.color_);