Commit 3442d663 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Resolve "Adapt to const nonconst new API"

parent fbbdb2f1
...@@ -173,9 +173,9 @@ int main(int argc, char** argv) ...@@ -173,9 +173,9 @@ int main(int argc, char** argv)
camera->process(image); camera->process(image);
// solve only when new KFs are added // 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::string report = solver->solve(wolf::SolverManager::ReportVerbosity::BRIEF);
std::cout << report << std::endl; std::cout << report << std::endl;
if (number_of_KFs > 5) if (number_of_KFs > 5)
......
...@@ -64,11 +64,48 @@ class CaptureImage : public CaptureBase ...@@ -64,11 +64,48 @@ class CaptureImage : public CaptureBase
void setDescriptors(const cv::Mat &_descriptors); void setDescriptors(const cv::Mat &_descriptors);
void setKeypoints(const std::vector<cv::KeyPoint>& _keypoints); void setKeypoints(const std::vector<cv::KeyPoint>& _keypoints);
cv::Mat& getDescriptors(); cv::Mat& getDescriptors();
const std::vector<cv::KeyPoint>& getKeypoints() const;
std::vector<cv::KeyPoint>& getKeypoints(); std::vector<cv::KeyPoint>& getKeypoints();
void setGlobalDescriptor(const cv::Mat &_global_descriptor); void setGlobalDescriptor(const cv::Mat &_global_descriptor);
cv::Mat& getGlobalDescriptor(); 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 } // namespace wolf
#endif // CAPTURE_IMAGE_H #endif // CAPTURE_IMAGE_H
...@@ -103,9 +103,9 @@ inline FactorAhp::FactorAhp(const FeatureBasePtr& _ftr_ptr, ...@@ -103,9 +103,9 @@ inline FactorAhp::FactorAhp(const FeatureBasePtr& _ftr_ptr,
inline Eigen::VectorXd FactorAhp::expectation() const inline Eigen::VectorXd FactorAhp::expectation() const
{ {
FrameBasePtr frm_current = getFeature()->getCapture()->getFrame(); auto frm_current = getFeature()->getCapture()->getFrame();
FrameBasePtr frm_anchor = getFrameOther(); auto frm_anchor = getFrameOther();
LandmarkBasePtr lmk = getLandmarkOther(); auto lmk = getLandmarkOther();
const Eigen::MatrixXd frame_current_pos = frm_current->getP()->getState(); const Eigen::MatrixXd frame_current_pos = frm_current->getP()->getState();
const Eigen::MatrixXd frame_current_ori = frm_current->getO()->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, ...@@ -151,7 +151,7 @@ inline void FactorAhp::expectation(const T* const _current_frame_p,
TransformType T_R0_C0 = t_r0_c0 * q_r0_c0; TransformType T_R0_C0 = t_r0_c0 * q_r0_c0;
// current robot to current camera transform // 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>()); Translation<T, 3> t_r1_c1 (current_capture->getSensorP()->getState().cast<T>());
Quaterniond q_r1_c1_s(Eigen::Vector4d(current_capture->getSensorO()->getState())); Quaterniond q_r1_c1_s(Eigen::Vector4d(current_capture->getSensorO()->getState()));
Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>(); Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
......
...@@ -101,9 +101,9 @@ inline FactorPixelHp::FactorPixelHp(const FeatureBasePtr& _ftr_ptr, ...@@ -101,9 +101,9 @@ inline FactorPixelHp::FactorPixelHp(const FeatureBasePtr& _ftr_ptr,
inline Eigen::VectorXd FactorPixelHp::expectation() const inline Eigen::VectorXd FactorPixelHp::expectation() const
{ {
FrameBasePtr frm = getFeature()->getCapture()->getFrame(); auto frm = getFeature()->getCapture()->getFrame();
SensorBasePtr sen = getFeature()->getCapture()->getSensor(); auto sen = getFeature()->getCapture()->getSensor();
LandmarkBasePtr lmk = getLandmarkOther(); auto lmk = getLandmarkOther();
const Eigen::MatrixXd frame_pos = frm->getP()->getState(); const Eigen::MatrixXd frame_pos = frm->getP()->getState();
const Eigen::MatrixXd frame_ori = frm->getO()->getState(); const Eigen::MatrixXd frame_ori = frm->getO()->getState();
......
...@@ -44,16 +44,17 @@ class FactorTrifocal : public FactorAutodiff<FactorTrifocal, 3, 3, 4, 3, 4, 3, 4 ...@@ -44,16 +44,17 @@ class FactorTrifocal : public FactorAutodiff<FactorTrifocal, 3, 3, 4, 3, 4, 3, 4
/** \brief Class constructor /** \brief Class constructor
*/ */
FactorTrifocal(const FeatureBasePtr& _feature_1_ptr, FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
const FeatureBasePtr& _feature_2_ptr, const FeatureBasePtr& _feature_2_ptr,
const FeatureBasePtr& _feature_own_ptr, const FeatureBasePtr& _feature_own_ptr,
const ProcessorBasePtr& _processor_ptr, const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status); FactorStatus _status);
/** \brief Class Destructor /** \brief Class Destructor
*/ */
~FactorTrifocal() override; ~FactorTrifocal() override;
FeatureBaseConstPtr getFeaturePrev() const;
FeatureBasePtr getFeaturePrev(); FeatureBasePtr getFeaturePrev();
const Vector3d& getPixelCanonical3() const const Vector3d& getPixelCanonical3() const
...@@ -169,10 +170,10 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr, ...@@ -169,10 +170,10 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
FactorAutodiff( "TRIFOCAL PLP", FactorAutodiff( "TRIFOCAL PLP",
TOP_GEOM, TOP_GEOM,
_feature_own_ptr, _feature_own_ptr,
nullptr, FrameBasePtrList(),
nullptr, CaptureBasePtrList(),
_feature_2_ptr, //< this sets feature 2 (the one between the oldest and the newest) FeatureBasePtrList({_feature_2_ptr, _feature_1_ptr}), //< this sets feature 2 (the one between the oldest and the newest)
nullptr, LandmarkBasePtrList(),
_processor_ptr, _processor_ptr,
_apply_loss_function, _apply_loss_function,
_status, _status,
...@@ -189,7 +190,7 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr, ...@@ -189,7 +190,7 @@ FactorTrifocal::FactorTrifocal(const FeatureBasePtr& _feature_1_ptr,
sqrt_information_upper(Matrix3d::Zero()) sqrt_information_upper(Matrix3d::Zero())
{ {
// First add feature_1_ptr to the list of features (because the constructor FeatureAutodiff did not do so) // 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 // Store some geometry elements
Matrix3d K_inv = camera_ptr_->getIntrinsicMatrix().inverse(); Matrix3d K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
......
...@@ -51,8 +51,10 @@ class LandmarkAhp : public LandmarkBase ...@@ -51,8 +51,10 @@ class LandmarkAhp : public LandmarkBase
const cv::Mat& getCvDescriptor() const; const cv::Mat& getCvDescriptor() const;
void setCvDescriptor(const cv::Mat& _descriptor); void setCvDescriptor(const cv::Mat& _descriptor);
const FrameBasePtr getAnchorFrame () const; FrameBaseConstPtr getAnchorFrame () const;
const SensorBasePtr getAnchorSensor() const; FrameBasePtr getAnchorFrame ();
SensorBaseConstPtr getAnchorSensor() const;
SensorBasePtr getAnchorSensor();
void setAnchorFrame (FrameBasePtr _anchor_frame ); void setAnchorFrame (FrameBasePtr _anchor_frame );
void setAnchorSensor (SensorBasePtr _anchor_sensor); void setAnchorSensor (SensorBasePtr _anchor_sensor);
...@@ -79,7 +81,12 @@ inline void LandmarkAhp::setCvDescriptor(const cv::Mat& _descriptor) ...@@ -79,7 +81,12 @@ inline void LandmarkAhp::setCvDescriptor(const cv::Mat& _descriptor)
cv_descriptor_ = _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_; return anchor_frame_;
} }
...@@ -89,7 +96,12 @@ inline void LandmarkAhp::setAnchorFrame(FrameBasePtr _anchor_frame) ...@@ -89,7 +96,12 @@ inline void LandmarkAhp::setAnchorFrame(FrameBasePtr _anchor_frame)
anchor_frame_ = _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_; return anchor_sensor_;
} }
......
...@@ -224,29 +224,35 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature ...@@ -224,29 +224,35 @@ class ProcessorBundleAdjustment : public ProcessorTrackerFeature
/** /**
* \brief Return Image for debug purposes * \brief Return Image for debug purposes
*/ */
cv::Mat getImageDebug() const; const cv::Mat& getImageDebug() const;
/** /**
* \brief Return list of Features tracked in a Capture * \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 * \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_; 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); 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(); return getProblem()->getMap()->getLandmarkList();
} }
......
...@@ -186,6 +186,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature ...@@ -186,6 +186,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*/ */
void establishFactors() override; void establishFactors() override;
CaptureBaseConstPtr getPrevOrigin() const;
CaptureBasePtr getPrevOrigin(); CaptureBasePtr getPrevOrigin();
bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last); bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
...@@ -212,6 +213,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature ...@@ -212,6 +213,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
const Matrix2d& pixelCov() const; const Matrix2d& pixelCov() const;
}; };
inline CaptureBaseConstPtr ProcessorTrackerFeatureTrifocal::getPrevOrigin() const
{
return prev_origin_ptr_;
}
inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOrigin() inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOrigin()
{ {
return prev_origin_ptr_; return prev_origin_ptr_;
......
...@@ -42,39 +42,4 @@ CaptureImage::~CaptureImage() ...@@ -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 } // namespace wolf
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment