diff --git a/demos/demo_processor_bundle_adjustment.cpp b/demos/demo_processor_bundle_adjustment.cpp
index d7b3affbaabd67b3484ce89b492213916755a964..5a13abcfb91f6bdac694aabdbba3f1f0b8fa71a7 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)
diff --git a/include/vision/capture/capture_image.h b/include/vision/capture/capture_image.h
index 367d230e8710aa945b7fe462fc32d0942a244c54..380d8db199aa47123907cef3890fc9c5ec2d3661 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<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
diff --git a/include/vision/factor/factor_ahp.h b/include/vision/factor/factor_ahp.h
index 4c47c0912e357b79cc2728c1cedb4555467e227b..833a3f3c34fb58856af214444f78a172d51b3225 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, 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>();
diff --git a/include/vision/factor/factor_pixel_hp.h b/include/vision/factor/factor_pixel_hp.h
index 0f7e18ed2708467dafab8655303c7a97f49bbb15..9df111684ec5d860d05a570b47f13d7bf502bacb 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 646a7fe664475eaac7d23be4617d0c95099866fb..b2cf8514edcfef30f093d8f3c29d3f8a50114076 100644
--- a/include/vision/factor/factor_trifocal.h
+++ b/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();
diff --git a/include/vision/landmark/landmark_ahp.h b/include/vision/landmark/landmark_ahp.h
index 14fd65b2befe4fd92653cd213d544d7a1586e250..ed07c5d7ee650a6a33ebf65f00f560fc419f08a4 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_bundle_adjustment.h b/include/vision/processor/processor_bundle_adjustment.h
index f19001ae44678be64ae2d594681bd72f2521654d..14ec7df6a92b9290faed246184624aa10a93dc0b 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<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();
}
diff --git a/include/vision/processor/processor_tracker_feature_trifocal.h b/include/vision/processor/processor_tracker_feature_trifocal.h
index 17b6fb8373e557f90482676a60493d0ce472fd00..73fca4400e3c1a426a2c0236f4f052bbacdbaa8c 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_;
diff --git a/src/capture/capture_image.cpp b/src/capture/capture_image.cpp
index 539e2fe329dd58518d93ef9338421aa9b756bc6d..5b3b2bb3fd4f67cd73d4ef9a07ea2fcaa1ffaed4 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<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