Commit 5ac8fc44 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

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

Resolve "Adapt to const nonconst new API"

Closes #20

See merge request !30
parents fbbdb2f1 3442d663
......@@ -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)
......
......@@ -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
......@@ -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>();
......
......@@ -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();
......
......@@ -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();
......
......@@ -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_;
}
......
......@@ -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();
}
......
......@@ -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_;
......
......@@ -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
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