Merge branch '20-adapt-to-const-nonconst-new-api' into 'devel'

Resolve "Adapt to const nonconst new API"

Closes #20

See merge request !30

demos/demo_processor_bundle_adjustment.cpp

@@ -173,9 +173,9 @@ int main(int argc, char** argv)
         camera->process(image);
 
         // solve only when new KFs are added
-        if (problem->getTrajectory()->getFrameMap().size() > number_of_KFs)
+        if (problem->getTrajectory()->size() > number_of_KFs)
         {
-            number_of_KFs = problem->getTrajectory()->getFrameMap().size();
+            number_of_KFs = problem->getTrajectory()->size();
             std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
             std::cout << report << std::endl;
             if (number_of_KFs > 5)

include/vision/capture/capture_image.h

@@ -64,11 +64,48 @@ class CaptureImage : public CaptureBase
         void setDescriptors(const cv::Mat &_descriptors);
         void setKeypoints(const std::vector<cv::KeyPoint>& _keypoints);
         cv::Mat& getDescriptors();
+        const std::vector<cv::KeyPoint>& getKeypoints() const;
         std::vector<cv::KeyPoint>& getKeypoints();
         void setGlobalDescriptor(const cv::Mat &_global_descriptor);
         cv::Mat& getGlobalDescriptor();
 };
 
+
+inline const cv::Mat& CaptureImage::getImage() const
+{
+    return frame_.getImage();
+}
+
+inline void CaptureImage::setDescriptors(const cv::Mat& _descriptors)
+{
+    frame_.setDescriptors(_descriptors);
+}
+
+inline void CaptureImage::setKeypoints(const std::vector<cv::KeyPoint> &_keypoints)
+{
+    frame_.setKeyPoints(_keypoints);
+}
+
+inline cv::Mat& CaptureImage::getDescriptors()
+{
+    return frame_.getDescriptors();
+}
+
+inline std::vector<cv::KeyPoint>& CaptureImage::getKeypoints()
+{
+    return frame_.getKeyPoints();
+}
+
+inline void CaptureImage::setGlobalDescriptor(const cv::Mat& _global_descriptor)
+{
+    global_descriptor_ = _global_descriptor;
+}
+
+inline cv::Mat& CaptureImage::getGlobalDescriptor()
+{
+    return global_descriptor_;
+}
+
 } // namespace wolf
 
 #endif // CAPTURE_IMAGE_H

include/vision/factor/factor_ahp.h

@@ -103,9 +103,9 @@ inline FactorAhp::FactorAhp(const FeatureBasePtr&   _ftr_ptr,
 
 inline Eigen::VectorXd FactorAhp::expectation() const
 {
-    FrameBasePtr frm_current = getFeature()->getCapture()->getFrame();
-    FrameBasePtr frm_anchor  = getFrameOther();
-    LandmarkBasePtr lmk      = getLandmarkOther();
+    auto frm_current = getFeature()->getCapture()->getFrame();
+    auto frm_anchor  = getFrameOther();
+    auto lmk         = getLandmarkOther();
 
     const Eigen::MatrixXd frame_current_pos = frm_current->getP()->getState();
     const Eigen::MatrixXd frame_current_ori = frm_current->getO()->getState();
@@ -151,7 +151,7 @@ inline void FactorAhp::expectation(const T* const _current_frame_p,
     TransformType       T_R0_C0 = t_r0_c0 * q_r0_c0;
 
     // current robot to current camera transform
-    CaptureBasePtr      current_capture = this->getFeature()->getCapture();
+    auto                current_capture = this->getFeature()->getCapture();
     Translation<T, 3>   t_r1_c1  (current_capture->getSensorP()->getState().cast<T>());
     Quaterniond         q_r1_c1_s(Eigen::Vector4d(current_capture->getSensorO()->getState()));
     Quaternion<T>       q_r1_c1 = q_r1_c1_s.cast<T>();

include/vision/factor/factor_pixel_hp.h

@@ -101,9 +101,9 @@ inline FactorPixelHp::FactorPixelHp(const FeatureBasePtr&   _ftr_ptr,
 
 inline Eigen::VectorXd FactorPixelHp::expectation() const
 {
-    FrameBasePtr frm = getFeature()->getCapture()->getFrame();
-    SensorBasePtr sen  = getFeature()->getCapture()->getSensor();
-    LandmarkBasePtr lmk      = getLandmarkOther();
+    auto frm  = getFeature()->getCapture()->getFrame();
+    auto sen  = getFeature()->getCapture()->getSensor();
+    auto lmk  = getLandmarkOther();
 
     const Eigen::MatrixXd frame_pos = frm->getP()->getState();
     const Eigen::MatrixXd frame_ori = frm->getO()->getState();

include/vision/factor/factor_trifocal.h

@@ -44,16 +44,17 @@ class FactorTrifocal : public FactorAutodiff<FactorTrifocal, 3, 3, 4, 3, 4, 3, 4
         /** \brief Class constructor
          */
         FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
-                               const FeatureBasePtr& _feature_2_ptr,
-                               const FeatureBasePtr& _feature_own_ptr,
-                               const ProcessorBasePtr& _processor_ptr,
-                               bool _apply_loss_function,
-                               FactorStatus _status);
+                       const FeatureBasePtr& _feature_2_ptr,
+                       const FeatureBasePtr& _feature_own_ptr,
+                       const ProcessorBasePtr& _processor_ptr,
+                       bool _apply_loss_function,
+                       FactorStatus _status);
 
         /** \brief Class Destructor
          */
         ~FactorTrifocal() override;
 
+        FeatureBaseConstPtr getFeaturePrev() const;
         FeatureBasePtr getFeaturePrev();
 
         const Vector3d& getPixelCanonical3() const
@@ -169,10 +170,10 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
         FactorAutodiff( "TRIFOCAL PLP",
                         TOP_GEOM,
                         _feature_own_ptr,
-                        nullptr,
-                        nullptr,
-                        _feature_2_ptr, //< this sets feature 2 (the one between the oldest and the newest)
-                        nullptr,
+                        FrameBasePtrList(),
+                        CaptureBasePtrList(),
+                        FeatureBasePtrList({_feature_2_ptr, _feature_1_ptr}), //< this sets feature 2 (the one between the oldest and the newest)
+                        LandmarkBasePtrList(),
                         _processor_ptr,
                         _apply_loss_function,
                         _status,
@@ -189,7 +190,7 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
         sqrt_information_upper(Matrix3d::Zero())
 {
     // First add feature_1_ptr to the list of features (because the constructor FeatureAutodiff did not do so)
-    if (_feature_1_ptr) feature_other_list_.push_back(_feature_1_ptr);
+    //if (_feature_1_ptr) feature_other_list_.push_back(_feature_1_ptr);
 
     // Store some geometry elements
     Matrix3d K_inv          = camera_ptr_->getIntrinsicMatrix().inverse();

include/vision/landmark/landmark_ahp.h

@@ -51,8 +51,10 @@ class LandmarkAhp : public LandmarkBase
         const cv::Mat& getCvDescriptor() const;
         void setCvDescriptor(const cv::Mat& _descriptor);
 
-        const FrameBasePtr  getAnchorFrame () const;
-        const SensorBasePtr getAnchorSensor() const;
+        FrameBaseConstPtr  getAnchorFrame () const;
+        FrameBasePtr  getAnchorFrame ();
+        SensorBaseConstPtr getAnchorSensor() const;
+        SensorBasePtr getAnchorSensor();
 
         void setAnchorFrame  (FrameBasePtr  _anchor_frame );
         void setAnchorSensor (SensorBasePtr _anchor_sensor);
@@ -79,7 +81,12 @@ inline void LandmarkAhp::setCvDescriptor(const cv::Mat& _descriptor)
     cv_descriptor_ = _descriptor;
 }
 
-inline const FrameBasePtr LandmarkAhp::getAnchorFrame() const
+inline FrameBaseConstPtr LandmarkAhp::getAnchorFrame() const
+{
+    return anchor_frame_;
+}
+
+inline FrameBasePtr LandmarkAhp::getAnchorFrame() 
 {
     return anchor_frame_;
 }
@@ -89,7 +96,12 @@ inline void LandmarkAhp::setAnchorFrame(FrameBasePtr _anchor_frame)
     anchor_frame_ = _anchor_frame;
 }
 
-inline const SensorBasePtr LandmarkAhp::getAnchorSensor() const
+inline SensorBaseConstPtr LandmarkAhp::getAnchorSensor() const
+{
+    return anchor_sensor_;
+}
+
+inline SensorBasePtr LandmarkAhp::getAnchorSensor()
 {
     return anchor_sensor_;
 }

include/vision/processor/processor_bundle_adjustment.h

@@ -224,29 +224,35 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
         /**
          * \brief Return Image for debug purposes
          */
-        cv::Mat getImageDebug() const;
+        const cv::Mat& getImageDebug() const;
 
         /**
          * \brief Return list of Features tracked in a Capture
          */
-        std::list<FeatureBasePtr> trackedFeatures(const CaptureBasePtr& _capture_ptr) const;
+        FeatureBasePtrList trackedFeatures(const CaptureBasePtr& _capture_ptr);
         /**
         * \brief Return list of Landmarks
         */
-        std::list<LandmarkBasePtr> currentLandmarks() const;
+        LandmarkBaseConstPtrList currentLandmarks() const;
+        LandmarkBasePtrList currentLandmarks();
 };
 
-inline cv::Mat ProcessorBundleAdjustment::getImageDebug() const
+inline const cv::Mat& ProcessorBundleAdjustment::getImageDebug() const
 {
     return image_debug_;
 }
 
-inline std::list<FeatureBasePtr> ProcessorBundleAdjustment::trackedFeatures(const CaptureBasePtr& _capture_ptr) const
+inline FeatureBasePtrList ProcessorBundleAdjustment::trackedFeatures(const CaptureBasePtr& _capture_ptr)
 {
 	return track_matrix_.snapshotAsList(_capture_ptr);
 }
 
-inline std::list<LandmarkBasePtr> ProcessorBundleAdjustment::currentLandmarks() const
+inline LandmarkBaseConstPtrList ProcessorBundleAdjustment::currentLandmarks() const
+{
+	return getProblem()->getMap()->getLandmarkList();
+}
+
+inline LandmarkBasePtrList ProcessorBundleAdjustment::currentLandmarks()
 {
 	return getProblem()->getMap()->getLandmarkList();
 }

include/vision/processor/processor_tracker_feature_trifocal.h

@@ -186,6 +186,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
          */
         void establishFactors() override;
 
+        CaptureBaseConstPtr getPrevOrigin() const;
         CaptureBasePtr getPrevOrigin();
 
         bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
@@ -212,6 +213,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
         const Matrix2d& pixelCov() const;
 };
 
+inline CaptureBaseConstPtr ProcessorTrackerFeatureTrifocal::getPrevOrigin() const
+{
+    return prev_origin_ptr_;
+}
+
 inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOrigin()
 {
     return prev_origin_ptr_;

src/capture/capture_image.cpp

@@ -42,39 +42,4 @@ CaptureImage::~CaptureImage()
     //
 }
 
-const cv::Mat& CaptureImage::getImage() const
-{
-    return frame_.getImage();
-}
-
-void CaptureImage::setDescriptors(const cv::Mat& _descriptors)
-{
-    frame_.setDescriptors(_descriptors);
-}
-
-void CaptureImage::setKeypoints(const std::vector<cv::KeyPoint> &_keypoints)
-{
-    frame_.setKeyPoints(_keypoints);
-}
-
-cv::Mat& CaptureImage::getDescriptors()
-{
-    return frame_.getDescriptors();
-}
-
-std::vector<cv::KeyPoint>& CaptureImage::getKeypoints()
-{
-    return frame_.getKeyPoints();
-}
-
-void CaptureImage::setGlobalDescriptor(const cv::Mat& _global_descriptor)
-{
-    global_descriptor_ = _global_descriptor;
-}
-
-cv::Mat& CaptureImage::getGlobalDescriptor()
-{
-    return global_descriptor_;
-}
-
 } // namespace wolf