From b83f4af09e0fb89801a8563b0f8ff610646a2c31 Mon Sep 17 00:00:00 2001 From: jvallve Date: Mon, 11 Apr 2022 19:04:25 +0200 Subject: [PATCH 1/3] trajectory size --- demos/demo_processor_bundle_adjustment.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp index d7b3affb..5a13abcf 100644 --- a/demos/demo_processor_bundle_adjustment.cpp +++ b/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) -- GitLab From 2d9776024c932ff38eab7bf58c2e921a77f082ab Mon Sep 17 00:00:00 2001 From: jvallve Date: Mon, 11 Apr 2022 20:44:57 +0200 Subject: [PATCH 2/3] wip --- include/vision/capture/capture_image.h | 37 +++++++++++++++++++ include/vision/factor/factor_ahp.h | 8 ++-- include/vision/factor/factor_pixel_hp.h | 6 +-- include/vision/factor/factor_trifocal.h | 21 ++++++----- .../processor/processor_bundle_adjustment.h | 18 ++++++--- src/capture/capture_image.cpp | 35 ------------------ 6 files changed, 67 insertions(+), 58 deletions(-) diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h index 367d230e..380d8db1 100644 --- a/include/vision/capture/capture_image.h +++ b/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& _keypoints); cv::Mat& getDescriptors(); + const std::vector& getKeypoints() const; std::vector& 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 &_keypoints) +{ + frame_.setKeyPoints(_keypoints); +} + +inline cv::Mat& CaptureImage::getDescriptors() +{ + return frame_.getDescriptors(); +} + +inline std::vector& 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 diff --git a/include/vision/factor/factor_ahp.h b/include/vision/factor/factor_ahp.h index 4c47c091..833a3f3c 100644 --- a/include/vision/factor/factor_ahp.h +++ b/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_r1_c1 (current_capture->getSensorP()->getState().cast()); Quaterniond q_r1_c1_s(Eigen::Vector4d(current_capture->getSensorO()->getState())); Quaternion q_r1_c1 = q_r1_c1_s.cast(); diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h index 0f7e18ed..9df11168 100644 --- a/include/vision/factor/factor_pixel_hp.h +++ b/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(); diff --git a/include/vision/factor/factor_trifocal.h b/include/vision/factor/factor_trifocal.h index 646a7fe6..b2cf8514 100644 --- a/include/vision/factor/factor_trifocal.h +++ b/include/vision/factor/factor_trifocal.h @@ -44,16 +44,17 @@ class FactorTrifocal : public FactorAutodiffgetIntrinsicMatrix().inverse(); diff --git a/include/vision/processor/processor_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h index f19001ae..14ec7df6 100644 --- a/include/vision/processor/processor_bundle_adjustment.h +++ b/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 trackedFeatures(const CaptureBasePtr& _capture_ptr) const; + FeatureBasePtrList trackedFeatures(const CaptureBasePtr& _capture_ptr); /** * \brief Return list of Landmarks */ - std::list 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 ProcessorBundleAdjustment::trackedFeatures(const CaptureBasePtr& _capture_ptr) const +inline FeatureBasePtrList ProcessorBundleAdjustment::trackedFeatures(const CaptureBasePtr& _capture_ptr) { return track_matrix_.snapshotAsList(_capture_ptr); } -inline std::list ProcessorBundleAdjustment::currentLandmarks() const +inline LandmarkBaseConstPtrList ProcessorBundleAdjustment::currentLandmarks() const +{ + return getProblem()->getMap()->getLandmarkList(); +} + +inline LandmarkBasePtrList ProcessorBundleAdjustment::currentLandmarks() { return getProblem()->getMap()->getLandmarkList(); } diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp index 539e2fe3..5b3b2bb3 100644 --- a/src/capture/capture_image.cpp +++ b/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 &_keypoints) -{ - frame_.setKeyPoints(_keypoints); -} - -cv::Mat& CaptureImage::getDescriptors() -{ - return frame_.getDescriptors(); -} - -std::vector& 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 -- GitLab From 5646ae7d3e20e7b125c6c6532e7179e0cffb6feb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= Date: Tue, 3 May 2022 13:00:28 +0200 Subject: [PATCH 3/3] wip --- include/vision/landmark/landmark_ahp.h | 20 +++++++++++++++---- .../processor_tracker_feature_trifocal.h | 6 ++++++ 2 files changed, 22 insertions(+), 4 deletions(-) diff --git a/include/vision/landmark/landmark_ahp.h b/include/vision/landmark/landmark_ahp.h index 14fd65b2..ed07c5d7 100644 --- a/include/vision/landmark/landmark_ahp.h +++ b/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_; } diff --git a/include/vision/processor/processor_tracker_feature_trifocal.h b/include/vision/processor/processor_tracker_feature_trifocal.h index 17b6fb83..73fca440 100644 --- a/include/vision/processor/processor_tracker_feature_trifocal.h +++ b/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_; -- GitLab